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SYNOPSIS 

The present work, proposes algorithms for finding collision 
free paths for a rigid, polygonal or a circular object moving thron 
^ace that is cluttered with polygonal obstacles. Ihe path can 
include rotations of the object. Two algorithms# one based on 
the Penally Function Approach, and the other based on the confi- 
guration space approach are have been proposed. 

The algorithm based on the Penadty Function i^proaeh, is 
an improvident on those proposed earlier by researchers# in that 
(i) it calculates the path directly in terms of the ^^int coordi- 
nates of the robot under consideration and (ii) it takes into 
accoimt the escact ehapes of the object and the obstacles into 
consideration for confuting the penalty for nearing the obstacles* 

The algorithm based on the configuration space i^proadh# 
uses a more efficient algorithm for searching l^xough the free 
sgpace for a path# than l^ie earlier algorithms proposed by ottwers* 
This algorithm moiks by starting with a strai^t line paldi betweisi 
the end points and then recursively modifying it to finally arrive 
at a collision free path* 


CHAPTER I 


INTRODUCTION 


1.1 Introduction 

Robots are destined to play a significant role in liie 
industrial scene of the future. This is primarily because 
they can be applied to a large variety of tasks# Most of 
the tasks which are potentially dangerous or sDnotonous, 
done earlier by human beings are now being t^en over by 
robots. 

Most of the tasks done by a human operator! need a 
certain emount of 'skill' achieved conscious or subconscious 
training. If robots are to perform similar taskSf some 
decision making capability is essential and this is provided 
by a computer. So most of the problaaos associated with 
is^arting 'skill' to the robotSf reduce to that of developing 
suitable algoritlBBS that can be implemimted on a eoi^uter* 
present work aims at developing algorithms for what is known 
as the 'Fin%ath Problm' • This problem can be stated ass 
givm m ob.leot with an initial position (location and ©rl^tsf- 
tlon)! a final positioii and a set of o^y^aietleii^,. located In , 
apace! find a continuous path for to oblect from to -initial 
to to final position! , that avoids collisions ‘wllli all to 
obstacles. The fin«%jath Wmlalm is asscciated with gross motion 
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planning and is the first problem to be solved in overall 
task planning. The other planning aspects, namely grasping 
planning and finemotion planning are not oonsid^ed h€a*e* 

1*2 Review of Previous Work 

Most of the algorithms proposed earlier, for solving 
the Fin(%)ath Problem, fall under the following three groiqss [ll* 

1, Hypothesize and test method 

2* Penalty function method 

3t Esqjlioit free space method 

1, Hypothesize and Teat Method* 

This was the earliest approach tried, towards solving 
the Fin<%}ath Problem, As the name itself suggests, the 
method is based on first proposing a path for 1die object from 
the initial to the final position, and then testing the path 
to see if it avoids eollisions with all the obstacles* If it 
does not, then, the path is modified to avoid the collisions* 
This is done repeatedly till the path obtained is free of all 
collisions, ^ the method basically relies on algorithms for 
(i) detecting possible collisions of an object, moving along a 
giveac path, with the obstacles and (ii) aodlfyizig m unsadTe 
path to yield one which is safe* Most of the algorltlWB (or 
heuristics) proposed [2] for the modification of an unsafe 
path are based on epprozimations of the shapes of the object' 
and the obstacles into regular geometric shapes* these 
ipproadiiatioiis are often drastic and hence methods ' based on 
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these fall to work, when the workspace is closely cluttered 
with obstacles. However, if the obstacles are sparsely 
located, these methods work quite efficiently because of 
their inherent simplicity* 

2 . Penalty Function Methods 

These methods are based on proposing a function of the 
position of the object, which when minimized, leads to tlm 
destination. The function value increases shaiply as the 
obiJect nears an obstacle , approaching Infinity whm the 
object collides with it# This provides a repulsive force 
which keeps the object away from the obstacle* Xn case these 
are many obstacles, then the penalties for nearing tech one 
of th««, are iuat added up to yield the resultant penalty# 

The minimization of the function could be done using 
the partial derivatives of the function with respect to Idtie 
configuration parameters# The sequence of values of the 
configuration parameters , obtained during minimization, repre- 
sents the seqiiusice of positions of the ob;}eot from its initial 
to final position and in turn the path of the ob^^t# 

The ma^or drawbacks of the penalty ftmction method are< 
(i) Ifost of these methods [3] use drastic ipproxlmations of 
the object and the obstacles with regular shapes such as circle 
square etc# or in the cast of 3 **^, by aphereSf cylindersi cubes 
etc* This r«iders it unreliable in many easesi and in. mM% 
others, paths idilch are actually feasiblet ruled out* 
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(ii) All the minimiaation methods guarantee convergence only 
to a local minimum because of the strictly local information 
that is made use of. Hence if there are minima (local) other 
than the destination, minimization might lead to one of these 
other minima. In such cases, since a minimum has been reached, 
no further progress can be made. So the algorithm will have 
to back track to one of the earlier configurations and resume 
the search in a diz^ctlon different from the one followed 
earlier. But identifying the points from where the search is 
to be resumed, might be very difficult. 

3. Explicit Free Space Methods 

These methods are based on building eapHclt represen- 
tations of those configurations of the robot that are free of 
collisions# set of these collision free configurations 
is also refer to as the free S^ace « The Fiik^ath Problem 
in this ease is that of finding a path throui^ the free space 
for the object. The different algorithms proposed, differ 
primarily in their characterization of the free epaee and the 
particular subsets of the free i^ace they consider for finding 
a path, Iczano-Perez £4] suggested an aXgorildaa which repre- 
sents the free space by thecoi^lesient of the space occtpied 
the obstacles, which have been expanded so as to allow the 
object to be shrunk to a point,. A reference point is chosen 
for the object. This is the point to which the object is 
shrunk* The Slgoritlai, then searches for a path for 'this 
reference point through l^e free spade* To do this# the .free 
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space is brtjken into polyhedral cells of varying resolutions 
and then a gri^h, whose nodes denote these cells and the edges 
represent the overlap between the cells, is formed* A path is 
found by searching through this graph* If the object to be 
moved is not convex, it is broken into a union of convex 
polygons and then the above algorithm is applied# 

Fig. 1, 3(a) shows an ob^Ject 'o', which is a union of two 
convex polygons P and Q, moving from a position 0 to position 
o'\ without rotations* * r' is its reference point and 0,, 0« 

iJL mm 

are the obstacles on the way* Fig* 1#3&) shows how this problem 
gets transformed to one where the point R is to be moved from 
R to R"'^ with the obstacles 0^^^, 0^^, 0^^ and 0^ on the way* 
Here 0^^ and 0^^ are the polygons obtained after ej^a^iding the 
obstacle 0^ to account for the polygons P and Q respectively. 

In case rotation is also allowed, a third dimension gets added 
to problem in 2-D* For a general motion in 3-D space , the 
configuration space becomes six dimensional* 

Another way of representing the free space was suj^ested 
by R*A, Brooks [53# Here, the free space is represaited at a 
union of generalised cones* These cones are In the form of 
corridors or pathways betwe^ adjacent obstacles* VaHd orien*' 
tations of the ob;}ect as it moves thixjugh a ge»»rSliaed eone 
are first detemlned and then a path, composed of si^ents of 
the axes of these cones is looked for* Th® geaeraUxed cones 
gaierated for a set of obstacles is shown in Fig* 1#2* 



Fi'g .I'lCcL) 
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The ma^or attraction of thefree space methods Is that, 
these algorithms will find a path, if one exists. Moreover, 
instead of ;3ust looking for a feasible path, some kind of 
optimization can also be incorporated allowing one to look 
for the shortest or the least eacpensive path. The disadvarfeage 
is that the computation of the free space might be very eaqpensive. 
However, these algorithms work very well, even when the obstacles 
are closely cluttered. For such closely cluttered workspaces, 
other algorithms either fail or spend ea undue amount of effort 
in path searching. 

Ifost of the ^orithaas Idiat have been pipposed for obstacle 
avoidance, are fundamentally tied to the use of object i^proxl'-' 
mations. These are inapplicable in cases where the ob;5®®t has 
to move very close to the obstacles, ^e algorithm suggested 
by Lozano-Perez and R,A, Brooks [6] is very good in this reject 
though it is limited to cartesian robots and is also computar- 
tlonally e3g>ensive# An efficient obstacle avoidance algorithm 
for general robots with i-^olute joints remains to be developed, 

1,3 Scope of the Present Work ? 

The present woilc proposes two algorithms, towards solving 
the Fin«^ath Probl«B, The first algoriliim is based on the penalty 
function approach. It incoxporates the following featurest 
(i) The path is generated directly in terms of the ^oint coordinates, 
iAma avoiding coi^utations to be carried out for the inverse 
tnmsformation from cartesian to 4oint apace , 



9 


(ii) Unlike previous work, using the penalty iunc-cion approach, 
the proposed algorithm takes into account, the exact shapes of 
the object and the obstacle concerned. 

The second algorithm is based on the configuration B|»ace 
^proach suggested by Losano-Perez [4], She subdivision 
algorittea suggested by Lozano-Perez and R*A, Bi-ooks [6] for 
searching tiirough the configuration ^ace, divides the whole 
free space into polyhedral cells and thai searches for a path 
through these* This involves, sKjre coB^utation than what is 
actually necessary because the cells which are uost significant 
are the ones close to obstacles. The algorithm proposed here, 
though is not based on subdivision of the free ^ace, concen- 
trates more on the neighbourhoods of the obstacles. This is 
©sqpected to reduce the amoimt of confutation needed and als© 
the storage ^ace required for the algorithm to be run* 



CHAPTER II 


FORMULATION OF THE ALGORITHMS 

2*1 Introduction 

Ihis chapter gives detailed descriptions of the algo- 
rithms developed along the lines sketched in Section 1*2* 

The algoritlan based on the Penalty Function Approach, is 
presented in Section 2*2* Subsequently, the algoritlmi based 
on the Configuration space Approach is discussed in Section 2*3« 

2*2 Penalty Function Approach 

This section describes a procedure to solve the fint^ath 
problem, using the Penalty Function Approach • Algorithms 
for calculating the penalties for nearing an obstacle, that 
take into account the exact shapes of the bodi^ concerned, 
are first presented* A penalty function method, that makes use 
of these algoritlsas, is then foxmilated* 

Vihen an ob^Ject is being moved through a mase of obstacles 
without colliding with any of them, pimalties are imposed for 
nearing any of the obstacles* Closer l^e object gets to m 
obstacle, the higher is the penalty it; receives# So the penal^ 
i^osed dapends essentially on the nearness of the object to 
an obstacle# For this, a way of cpantifying the nearness or 
closeness of an object to an obstacle needs to be found* 
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Before attempting to develop algorithms for doing this, we 
first define the ‘nearness' between two bodies. Let 0^ and ©2 
be the sets of points that constitute the two bodies. Let (X, Y) 
be the pair of points such that X e 0^ and Y e 0^, for which 
I I X- y| 1 (distance between the two) is a minimum. This distance 
denoted by d^.^, gives the smallest distance of separation or 
the 'nearness' between the two bodies. Algorithms for computing 
the for any pair of bodies, are now proposed. 

Ve confine ourselves to problems on 2-dimensions, i.e. 
problems which involve movement of a planar object, through a 
maze of obstacles which are also planar. We also assume that 
the object and the obstacles are either circles, or arbitrary 
polygons. With these, the simplest case that one can think of 
is that of a circular object approaching an obstacle, which is 
also circular. In this case, d^._ is given by d. = d„ - rT - r«, 

where r^ and r 2 are the radii of the object and the obstacle, 
respectively and d^ is the distance between the two centres. 

A problem, slightly more complicated than this is one 
where a circle approaches a polygon. 

A circle can hit a polygon in one of the following two 
wayss (i) One of the vertices of polygon could hit the circle 
or (ii) the circle could touch the polygon along one of the 
edges. These two cases have been illustrated in Fig. 2.1(a). 

The algorithm, that takes into account both these cases, 
is described below. Let and O 2 be the circle and the polygon 
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reject! vely, ^proaching each other* with R beiiig the centre 
of the circle, as shown in Fig, 2* 1 (b), A vertex of the 
polygon, is first chosen, fhe distances dV^ and dJS^ of the 
point R from the vertex and from the edge (clockwise 

edge starting from with respect to a point within the 
polygon), respectively, ai^ computed, A check is now made to 
see if the foot of the peipeadicular from R to falls 

within the edge, i.e,, between and ^4.3^* 1^ Is not so, 

then dEj^ is assigned an arbitrarily large value (as for dEj^ in 
Fig, 2.1(b)), This is done because, wh®Q the circle hits an edge 
of the polygon, the edge becomes tsoEigentisd to the circle with 
the point of tangency being obviously within the edge. The line 
joining the centre of the circle to the point of tangaacy, will 
then be perpendicular to the edge, H^nce if the foot of the 
perpendicular, dropped on an edge of a polygon, from the centre 
of a circle, falls outside the e<%e, th^ the edge is definitely 
not a potential fouling edge* 

The dEj^^s ^^i ® computed for every i, 1 » l,n 
where n is the number of vertices of the polygon. Let dE be 
the smallest of the dE^'s and let dV be the smallest of the dVg s« 
How the smaller of the two, dE and dV, becomes the for this 
pair C0|^ and 0^) of bodies. 

The most general problem is that of a polygon appxnachiiig 
s polygon* Ve first consider only convex polygons* The method 
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developed, here will later be extended to non-convex polygons 
as well* Here, again, we have two possible v/ays in which a 
collision could occur between a patir of polygonal bodies - 

(i) a vertex of one polygon hitting a vertex of the other, 

(ii) a vertex of one, touching an edge of the other* An 
algorithm, which takes into account both these, is described 
below* 

In Fig, 2m?., let 0^ and 0^ be the two polygons, under 
consideration, with ^2 their re^ectiw centres 

(these centres can be a rough estimate of the estact centres) • 

The Rj^'s <i - 1,2), can be located as points with coordinates 


given by, 


x?" 

( 


4 - 

7 




) 


where is the largest of the x-coordiaates of the vertices 
of 0^, and the smallest. Similarly, and refer 

to the maximum and minimiaa values, respectively, of the y^^wsordinates! 

I 

of the vertices* 

Having located and R^, the segment towh end 

this line segmwat is checked for intersections with the edges ^ 

bf Og, let v| v| (say md vj (ss^ E^) be the edges 

of Oji and Og re^ectively, that intersect ^8* 2,2, ! 

®1 “ ^1 ^ distances of the two «id-vertices 

of from Eg and those of Eg from sape first determined* Itoi, 
while t3?ylng to ccs^ute the distance of a vertex of one polygon 
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from an edge of the other, a test is made to ensure that the 
foot of the perpendicular from the vertex onto the edge falls 
within the edge. If this does not happen for any of the four 
distances (V^ and from E2 and vP and v| from E^^), then 
is simply the minimum of the distances between pairs of vertices, 
i.e. the minimum of |v^ vP^ |v^ vPjandjv^ V^, Otherwise, 

the minimum of the four vertex to edge distances, subject to the 
above mentioned condition, is found. Let this distance be d 
with the corresponding vertex and edge being and E^ respec- 
tively. With these as initial data, a search is now carried 
out to determine the 

The distance d C3f every successive vertex from 7 ^ along 
the direction V" V^, from the edge E2 is computed and d updated 
everytime until d stops decreasing or the foot of the perpendi- 
cular from a vertex falls outside £2* Let the last vertex, 
that satisfies these criteria be and let the edge incident 
on in the direction of search ( ) be V^. Let 

be the end vertex of Ep, in the direction opposite to that of 
V^, (i.e, if is clockwise, this direction is taken as 

anti-clockwise) where the directions are all with respect to a 
point within the corre^onding polygon, A similar search is 
now carried out, starting with the present value of d, the 
vertex v| and the edge V^, 

This whole process is repeated until d can not -be decreased 
further. The value of d thus obtained becomes between 

O^- end O2. 
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The above algorithms are quite efficient, in the sense 
that their complexities are not of a very high order* In the 
case involving a circle and a polygon, as is evidaht from the 
procedure outlined, the search involves going romd the list 
of vertices of the polygon, ^ust once, i*e* is of coapleadLty 
0(n) where n is the number of vertices of the polygon# For 
the case involving two-polygons, the worst case cou^jlexlty 
woiild be 0(n4m-4) where m and n are the niimber of sides of the 
two polygons. The average conQjlexity would be niuch less because, 
using the line ^ start the search serves as a very good 
heuristic and it shortwis the search considerably# 

In case, one of the two polygons is not convex, the non- 
convex polygon is broken vsp into parts, each of which is a convex 
polygon, i.e, if 0^^ is a non-convex, planar body, -Waien we split 
it up as, 



m U 



where is the ^-th convex coi^nent of the non-convex polygon 

and is the number of coagjonents* Assuming Og is the other 

polygon (d ^ 4 „ is to be computed for 0^^ and Og)^ <3^, which is 
the closest distance of approach between o|^ and Og, is computexl 
for all 4 ** 1# minimum of all these s gives the 

for On md 0«. 

A d. 
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We now formulate an otostacle avoldanoe algoritlm, 
baaed on the penalty function ;^proach, that maLces use of the 
algorithms given above. Any such algorithm* essentially 
produces a sequence of positions of the object to be moved, 
which defines the path that avoids collisions with the obstacles 
on the way. This sequence of positions can either be gen«*ated 
in terms of the cartesian coordinates or directly in terms of 
the 3oint-coordinates of the robot being used to execute the 
motion. Since, any motion, tobe executed by a robot, will 
ultimately have to be defined in terms of its ^oint-coordinates, 
a path defined in terms of the cartesian coordinates will have 
to be transformed to an equtval«it path in the ;|oint-coordinates, 
for it to be executed. This transformation, from the cartesian 
frame to the frame of the ^oint-coordinates, is computationally 
very esspfsasive. Generating a path directly in the ^oint-space, is 
although not as easy a task as doing it in the cartesiaz^-s^ace, 
it avoids the inverse-transformation to be carried out at every 
point along thecartesian-path if the path is defined in the 
cartesian-space. Here we take the tatter option, i,e« of 
generating a path directly in terms of the 4oint co-ordinates 
of the robot# 

The startii^ and the goal positions of the object to be 
Boved are first defined. This definition will obviously be 
in tezms of the cartesian coordinates because a position defined 
in terms of the doint-coordlnates cannot possibly be visualised. 
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The representation we adopt for this definition Is as follows. 

As shown in Fig, 2*3. a reference point is chosen for 
the ob;5ect* If the ob;3ect is a circle, its centre is chosen 
as its reference point R(Xq, y^) and if it is a polygon, some 
arbitrary point (preferably one close to its intuitive centre ) 
inside the polygon is chosen R(x^, ^o^ reference point* 

A local frame of reference, with its ojfigin at the reference 
point is th«a chosen* This local frame is fixed rigidly to 
the object* The ob;3ect is now described in terms of these 
local coordinates* If the object is a circle (0 with iAie local 
frame X;^, - y^,), this description would simply be the radius 
of the circle* If the object is a polygon (O with the local 
frame x^. yj, ), specifying the local coordinates of the 
vertices of the polygon, would be a complete description of 
the polygon* So, the position of an object cai be completely 
defined by ^eclfying the local frame of the object with respect 
to the global frame X-Y# In case the object is a polygon, 

Bpart from the coordinates of the origin of the local frame, 

Ihe ori natation of the local frame with respect to 36»Y win also 
have to be specified* This angle is d«ioted by 0*. As in Fig, 2, 3, 
the position of a circiklar object ©an therefore be defined as 
Fq) i»e* thecoordlnates of the origin of x - y and that 
of a polygonal object as y^, 0), 

To generate the path, directly in terms of the joint- 
coordinates, the starting and the goal positioas of the object, 
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defined in the oarteidan space » are first transformed to their 
equivalent ^oint-coordinates* This transformation from 
cartesianf-space to the ^oint-space, is speeifio to a partloular 
mechanismt and can in no vay be generalised* The sipecific 
meohanifes considered here is a 4»dc^ree of fTOcdom (5 d^rees 
of freedom if additional wrist roil for moiKiirciilcr objects 
is included) robotp the configuration of v/hioh is giv«a in 
Fig* 2*4. The procedure adopted heans for this transformation 
is due to Pam [7]. 

Let the 4oint«>aagles be ^1* ^4 for the 

aaditlwal rotation) aid let «» mk lengths be and 

The aa»s for the rotations ^4 normal 

to the plane of the omchanisBiD msd that for is paralim to 
the 2^e3d3* The aads of rotation for 9^ is along the link 4« 

The motion ti^es place on the 2r*y place* 

Let the local frames of refemce for each one of the 
links of the robot be as ^wn ixi Fig* 2*5* The object is held 
such that the origin of the frame attached to the md effector 
ix^ ** F4 * coincides with the refmnce point of the object* 
How, if CTj^ 3 i» th« matrix demeriptioii of the fjpeme 3^ “ *j[ 

with rtapect to the frame ** ®i«»i» 



Local Frames of rf’efenat\ce 4oT 
' Lmlts of the Man^ulatot 





a4 



vh^ve Cj « cos and m sHi i « 1*4, 

Th»TnttoT9t, ths from# with reis^eet to 3i^-yQ«*z tioiiQld 

be given by. 
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^ ^ 

0 C 1/^. 


0 0 0 1 


Cg 0 -Sg ^2®2 
0 10 0 

®2 ^ ^2 ^2^2 
0 0 0 1 


®3+4 ^ 


0 

■®3+4 
0 0 


1 

0 


S3+4 ^^4^3+4^ ^3^3 
0 0 

C5.14 ■"^4S344“^3®3 
0 1 



«"S-. 0 0 

JL 

Cl 0 0 

0 1 ^1 
0 0 1 


*^3+4^2 

0 

*•^3+4.2 

0 


0 

1 

0 

0 


®344-2 

0 0 

C3+V2 *"'^4®3+4-2*^3®3-2'^^2®2 
0 1 


®1®3+4-2 “®1 

®1®3*«4-.2 ®1 

“’®3+4*.2 ° 

0 0 


*^ 1 ® 3 + 4-2 ® 1 ^'^ 4 ® 3 +^ 2 '^^ 3 ® 3 - 2 '*'^ 2®2 ^ 

® 1 ® 3 + 4-2 ®1 ^‘^ 4 ® 3 + 4 - 2 ‘ 2 ^ 3 ^ 3 ^ 2 *'^ 2 ^ 2 ^ 

C344-2 *‘4®3+4-2*^3®>2’*^2®2'*‘A 

0 1 

C2a) 


Since the object (fixed to the end effector) i» to 
reiaain on the plane ifcile being moved, the origin of the 

^ oa a plane parallel to the x^ - 
plane, at a height h^ above it, aasumisg the thicioaess of the 
object is 2h^* From eqyti* (2#l), we have 


*4®344-2 "4®>2 * • 

K 

0 

(2ii2a) 

®1^^4®3-*4-2 ‘^^3®3-2 **^2^2^ * 

X 

(2*2b) 

®1^'^4®344-»2 ’*''^3®>2 *‘^1^2^ “ 

y 

(2.2e) 


and else 



26 


vAiere x and y are the i^ecified x and coordinates of the object 
on the Xq-Yq plane* How an additional condition, that the link 
4 should be normal to the x^-y^ plane, is iiaposed to avoid 
tilting of tile object* I'l'om Fig, 2*6 we have, 

©2 «• (©^ + ©^) ss "•%/2. 

i*e* ©j + ©^ •• ©2 ss 11/2 (2*3) 

therefore, cos (©^ + ©^ - ©g) ** ®3+4»«2 ^ ^ 

and sin (©^ + • ®3+4»2 * ^ 


^bstituting these into e<ps, (2*2a), (2*2b) and (2*2c), we 
have, 


•'^3®3-2 '^2®2 * 

^4-4 *^^o 

(2.4a) 


«» X 

(2,4b) 


* y 

(2*4c) 


S^m eqns. (2*4b) and (2.4c) we get, 

'^3^3-2 '^2*2 ** where r « 

From this and e<|a. (2.^a), we get, 


or, 


/g *1* + 2^2^5 Cj «s r^ + (^4 -ij. *** 


©, 


cos 


2^2 “^3 


Substitiiting this into (2«4a), -m get ©g. %e e:^ressdon for 
© 1 ^ cm also be obtained from e^^piKtions C2.4b) md C2«4c) m 
m Tan*^ (y/x)* How tiiat ©g and ©^ are laxnmi ©4 can be got 
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easily from the relatloft #5 'h- « %/?.* f-to rSvm the 

x-y coordinates of the oh^Ject, all tOie doint 54ici3.es 1 » 1»4 
can be coiaputed using the above procedure. If the object being 
mv^ is a polygeny where rotational oanoeuiiirlnfi is necessoaryt 
the rotation of the end effector, #5, can be easily obtained 
as « 0 4“ 0^, where 0 is the orientation of tiie obdeet and 
is soiae constant. 13ais is because# ^daile tlio change in 

Of th, ob3«t .ill bo the some ao tho chsoge in .5. 
the starting values of and 0 need not be the Bm&* 

With these# the ioint angles are calculated for the 
starting and the goal positions# md the ^olnt vesetors 

« i#3^ #25 #4 #g|® Cor 0^}^ if there is no 

sotatlon] are fomed. Ihus the algorith® starts idth the 

calculated ^^tart ^final coaiftites the successive 
intemediate positions in the ^int space as Tc till 

^ * ^findl* corre^onding cartesian points can be 
easily cesaputed by direct kinematics. 

Now out of the five ijoint angles# md #5 are ind^endent 
of Ihe rest* Ihe values of Sg# #5 aid are governed toy two 
m$m* C2.3 end 2 » 4 a). So only one of these tisree can toe 
treated as m indapiodent variable. Tbm in the dei»t vector# 
only 1 m of the cosponenta Car three if rotation of the object 
ie eiso mim into eocotnt} ere indepeetteit. fhe state of the 
rotoot can theveftore toe :hiny deftoed igr two«*diiaan^^ (or three) 
vector and theeeereh for itoi pa% toe eemed im vi^i this. 
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The other joint angles at any point on the path can be computed 
easily using eqns. (2.3) and (2.4a), From eqn* (2*3) vre have, 


e 


3 


©2 « “«4 


i.e» sin (©2 •• ©2) “ cos ©4, 

Substituting this in e<fi* (2.4a), we get, 

-^3 cos ©4 + ^2 ^2 ** ^^4 - + h^ 

If we treat ©g as an independent variable, we get 


©4 « cos 


and from ecp. (2*3), 






) (2.5) 


©3 «s + ©2 ©4 


( 2 . 6 ) 


Let the reduced joint vector, we use for searching th© path, 
be denoted with the corresponding reduced vectors lor 

the starting and final positions being and respec- 

tively. 

The penalty function approach is based on an objective 
function which is to be defined, over the range of -^e variables 
involved. This objective function, when minimized using some 
minimization algorithm should lead to the destinatioix which 
the object is supposed to reach, M objective function of the 
form 1^(1 * Cg P/f) Is chosen for this pnxpose* Ds is 

the norm of the ^distance ^ separating the object in its preset 
position £rm its destinatloa, %e word Wstaace' he^ 
used in a more general sense here, in that it accounts for the 
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difference in orientations as well, in case the motion involves 
rotational manoeuvring. The term Dgacts as a kind of pull 
towards the destination. The terms and are the penalty 
terms which prevent the oh^Ject from nearing an obstacle and 
the ijoint angles from going beyond the limits re^ectively* 

© 2 ^ and ag are constants (penalty parameters for P^ and P^ , 
rej^ectively), 3he procedure for evaluation of each one of these 
terms is described below, 

(i) Evaluation of ^ 

Let the reduced ^oint vector corres^jonding to some 
position of the object be Jt, Dgis now' given by 

Dg « I j I 

l.e. D - ( JC„ - rJG.)2 ]V2 

i«al * •*•* * 

where aaad are the i-th components of «ftd ^ 

re^ectively, 

(ii) Evaluation of P ^.i 

The presence of this term, prevents the obiJect from 
approaching any obstacle, too closely. Hence, the behaviour of 
this term should be such that it increases as Idue object neasfthe 
obstacle and the increase ahould be sharp when the object is 
close to the obstacle, X siaple function which would behave 
like this is X/ ^ 4 ^ where is the saallest disrtanee which 
separates the object end the obstacle, Ihls ^ 4 ^ is ecsputed, 
as described in V&e edgorithtas discussed earlier in this section. 
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Since can be calculated only in the cartesian 
£rmef ve will need tbe position of the object in the 
frme (see Fig« 2*5 )• ^is can be done directly t uain^ 
e(;gris» (2*6) # C2*4b) and (2«4c), once the reduced 

vector is known* The reduced doint vector v/ould be available 
at any point along the path because the path ^/ill be generated 
directly in tenaa of the reduced joint vector* 

The resultant p^salty function» for all the obstacles 
put together can be defined as* 

omiM - 

p « I . 

tdiaret ONUH la tha nunbar of obafeaelaa and d^^^ is the il^,n 
between theo object and the i**th obstacle* 

Here aosie saving in coapitation can be achieved by 
restricting the evaluation of the penalties for nearing the 
obstaclesf to a subset of the coai^lete set of obstacles* For 
any position of the objiot» the obstacles which have a sig^fi** 
cant effect on the subsequsat siotion of the object are only the 
ones which are sufficiently close to the present position of 
the object* Xn other words* tmly Idiose obstacles for which 
< S* ^ere S' is some aisalX fiiantityf need be tfdsisi into 
ccsisidesmtioa* So* a set of ofostsolts is dbmoleaily naintaliicd 
with obstacles being deleted from or sppei^yid to the set after 

every step which the object tahesf carding to the cimditioii 

i 

<$ • penalties are in tum evaluated only with respect 
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to those obstacles which belong to this set and the stammation 
^ ^/^in elements of this set, 

(iii) Evaluation of P^s 


Ihe rotation of most Joints in practice, v/ill necessarily 
have limits, beyond Tdiich the corresponding Joint angles cannot 
go. So any path which, while being traced, causes the Joint 
angles to cross these limits will be useless# Ihe term , 
keeps Joint angles within the prescribed limits by iiigjosiE^ 
a penalty for ^preaching the Joint angle iJLraits, 

1 i 

Let and be Ihe lower and upper limits of the 
i-th Joint angle. So if the i-th Joint angle has a value 
at some point along the path, we need to make sure that, 


min 


< 



< 


fii 

max 


or 0 < < 1, where 

A term of the form 
0 and 1 as required. So 
we have, 






max 


min'* 


1/^^ 4* l/(l*4^) would keep betwe^a 
for all the Joint angles together. 



nf 
m £ 

iwl 




) 


wh(«E*« nfls the niaiber of degrees of freedom of "^e robot being 
used# 

Here too, given the reduced Joint vector j the rest of 
the Joint angles and in tu» P^ can be evaluated# 
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Given the reduced joint vector at any point along the 
path, the value of the objective function Dg(l *** + ©2 

can now be evaluated, using the methods outlined above# So 
the objective function has effectively been defined as a function 
of the reduced joint vector* From what has been said above, 
about the evaluation of the terms and , it is clear 

that all the three terns are every^diere positive^ So if > 0 
and a 2 > 0> the value of the objective function will always 
be greater than or equal to zero# The only point ^ere the 
function value becomes zero is the destination since at this 
point 0, H^ce the destination, is where the global minimiM 
of the objective function lies# It follows that this function, 
when minimized in an unconstrained fashion would lead to the 
destination (provided there are no local minima on the way)* This 
minimization can be done using any of the hnmm optimization 
algorithms# The Davidon-Fletcher-Powell algorithm is the best 
known method that uses gradients, so initially that was decided 
upon for this task# After testing it on some problems, it was 
found that since a path th»ugh a maze of obstacles Involves 
fre(|ueat sharp turns, the takes too many cycles of c<»putaf 
tion at each turn, though it eventually does conver^ge to the 
minimum# Moreover since it is based on one-dimensional minimi- 
zation along every new direction of search chosaa, the palfe 
obtained ts noihtre near the ‘best* path to the destination. 

These factors proapted Ihe use of a rathar onide method in idiich 
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the gradient of the objective function is evaluated and then a 
small atep is taken along the negative of the gradient repeatedly 
till the minimum is reached. In this caset it did tom out to 
be more efficient than using DFP, because of the consequent 
reduction in the path length drastically and also the avoidance 
of a one-dimensional minimixation at every stage* 

Problems involving rotational manoeuvring pose an 
additional pK»blem, during minimization of the objective function. 
This is due to the presence of very small local minima with 
reject to the in plaiae rotation of the polygonal object. 

Referring to Fig. 2*7# let 0 be the object with R being 
its reference point aaad let 0^^ be the obstacle with its 
reference point, let the line through R, perpendicular to W of 
intersect the object and the obstacle at V and re^ectively. 
For siaq^ilicity Ist us assiaae that 0 and are rectangles with 
the side WW of 0 being parallel to side ot 0^, It is easily 
sem from Fig, 2,7 that for this case is which is 

nothing but 1 ^ 1 1* 1^1* if the orientation of the object 

is such that RW coincides with RVu^, we have 
Similarly If WJ coincides with RVj^, Since 

m\ < m w have - w > i^ii - mi •> 

or, 

^^Win ^ ^idn * 


^ ^\ia ** ^*Wn 
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This shows that for the position shown in Fig, 2.7» the penalty 
for nearing the obstacle 0^ is in fact a local nlniiMii with 
respect to the orientation, because the penalty increases 
along both the directions of rotation (U towards V and W towards 
V). This will prevent the ob;5®ct from rotating in any direc- 
tion. This problem is eliminated by adding some hypothetical 
inseribing cirtJles to the object. If the object is too elongated 
two inscribing circles at either end in the elongated direction 
are added. Otherwise, only one circle, the centre of which 
coincides with the reference point of the object is added. An 
illustration of this is given in Fig, 2,8, 

In each case shown in Fig, 2,8, 0 is the object, E Is 
its reference point and the 0^'s are ISti# Inscribing circles 
that have been added. The penalty for nearing an obstacle is 
now calculated as, 

^ ^ im: . 

( 1 /^) = (V<4„,p) ♦ ( l/<4in,3 5 

Where is the resultant ^neamess^ to the obstacle , 

*4iln,p ‘^in polygoned object and the obstacle, 

and j Is the d ^^ ^ between the j-th inscribed circle md 
the polygonal object, being the number of these inscribed 
circles* This additional suemation term in the penalty function, 
has the effect of ^msoothening^ out the small local minima which 
occur with reject to the inplane rotation of the object, as' 
ea^lained above* 
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Fig-E*8 


Hyf)otheticQLl Ii^sctlbing Circles 
tb 'Smoothe/Ti out' the. Objective 


being cuidecl 
Funcfciori 
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Minimization based on gradients is allov/able only if 

the objective function is differentiable everyi^ere in the 

range of the variables involved* So the differentiability 

of the function we have chosen, must be proved before any 

of these minimization algorithms based on gradients, can be 

used* The objective function we have chosaa is of the form 

2 ( 3 ) 2 > 1/9 

1^(1+ Pg + ffg P^). Ds being gives by [ S ~ 1 ' 

is obviously differentiable tilth reject to r^* 

is given by, 

n . _ 

£ + 

i»i 1 - 

vhere, 

^ ~ ^ 

i '' . 

‘'max ‘'min 

This too is differentiable with respect to ©very where 
except ©^ « and ©^ Since the T.vhole idea of 

having a term of this sort is to prevcast the joint angles from 
going beyond the limits, the joint angles would be confined 
to the range, 

«mln ‘ • '*•« is differentiable* 

Proving the differentiability of P^ is not a» straight forward 
as it was for D 5 and because we do not have my closed form 
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espresiion for this, The fonn have chosen for is 
where is as defined at the beginning of this section. 

Differentiability of a function of this form has been proved 
by Elmer G. Gilbert and Daniel W, Johnson [s]. Now, since all 
the three tezros and are differentiable, the complete 

objective function D/i + ♦ P a^) is also .Jlfferentiable 

over the range of the joiat angles, which constitute the 
variables for the present problem. 

2.5 The Configuration Space ^oroach 

C 5 

This approach is based on building a configuration space 
by expanding the obstacles, which will in turn enable us to treat 
the object to be moved as a point. Using the same repres^itation 
for objects and obstacles as described in Fig. 2.3 , the object 
is shrunk to a point at its reference point and the obstacles 
are eaqpandied accordingly. Now if the reference point of the 
object is always confined to the free j^ace, i.e. the complement 
of the space occt^pied by the obstacle, then collisions will not 
occur. So the problem of findijag the path for an object from 
one position to another reduces to that of finding one for a 
point through the free space in the presence of the e:^anded 
obstacles. %}ecifie algorithms for finding ihe path using this 
epproach are presented below. 

2.3.x 

Restricting otnrselvts to two dim^sions, Xet us first 
consider the pxt>blimi of moving a circular object idaich does not 
re^re any rotational manoeuvring through a maze of obstacles, 
all of Kdiich are polygonal. 



The first st€ 5 > is to esspand the obstacle so as to allow 
the object to be shrunk to a point (in this oase,^ the centre 
of the circular object). The algorithm for this is <|uite 
straight forward. 

Referring to Fig, 2.9# let be a vertex of the obstacle» 
the adjac€aat vertices being and Let 12ie object be of 
radius r. Asstaaing a margin of safety ‘hT is to be provided* 
let d *a m+r. Now if the object is shrunk to a point at its 
centre* then the distance between the point and any of Idiie 
edges of the obstacle cannot be less than d * if a collision 
is to be avoided (within the limits of safety ). So if the 
object is reduced to a point* each edge of the obstacle will 
have to be moved out by a distance V * Let the two edges 
oilginally incident on intersect at a new point V after 
being moved outward and let a and b be the feet Of 'the piarpen- 
diculars dropped onto the new positions of these ^es« It is 
easy to show that L ** d/sin fn/2) where -i) is the internal 
angle nt can be computed easily from the coordinates of 

Vj^* Vg and Vj* using the cosine rule as 

^ . eo.-l < dijAj'jh . ) (2.7) 

the vertices V. end It is clear from Fig» 2«-9 that the 

A 3 

line bisects angle 3Prom the coordinates of 

and Vj the slope of this line end hence the angle it makes with 
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the x-axis (global) can be easily determined* Let this eoigle 
be p. if (x, y) and (x^* y^^) are the coordinates of V and 

respectively* we have, 

X a + L cos p <2*8a) 

y » yj^ ♦ L sin p (2*8b) 

Therefore, given the coordinates of the vertices of the obstacle 
in the global frame, the coordinates of the vertices of the new 
ea^anded obstacle can be obtained from eqns* (2*8a) and (2.8b)# 

The above algorithm is of conpli^ty 0(n) ^diere'n is the 
number of vertices of the obstacle, since it is evideait from the 
procedure giv«a that the algorithm needs ^ust one pass around 
the list of vertices of the obstacle. 

Having expanded the obstacles, we now have to look for a 
path for a point through the free space# ihe algoritbia for this 
is presented b^ow# 

As shown in Fig,(2«10), let A and B be the startixig and 
goal points respectively# Let °1' Os and 0^ be the expanded 
obstacles# How the siaplest path from A to B is a straight 
line dcining the two, provided it is feasible# So, a straight 
line joining A and B is first dfawn and is checked for inter** 
sections with the obstacles. If there are no such intersections, 
then a path has been foimd# Otherwise, let On be the obstacle 

mt 

idiich comes first • on the way from A to B aloi^ Ihe 
stfwdght line# Let the two points of intersection of If with 
0^ be a fhd c« M&w a line (pq) perpendicular is drawn at b. 
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B 


Ffg-MO Seatcl. fof a Rxih bcWn Pomts A and B 




which is the mid point of ac* This line is searched for points 

C 

nearest to h on eithei* side of. the line AB and not lying 
within any of the obstacles. 1!hese two points have been marked 
as and Pg in Fig. 2*10, Each one of these two points would 
obviously be on the edge of some obstacle* So if we look for 
a path from A to B, passing through one of these two points P^ 
and Pg, we would be unnecessarily forcing the path to graze the 
boimdaries of the obstacles* Ihe points and P« are therefore 
offset from the obstacles* Let the new points obtained be 
and Cg, How the points and Pg are searched for and later 
offset to and Cg respectively, has been e35)lained a little 
later* Having obtained, and Cg, one of these tm, say is 
chosen, based on a criterion which has been e::^lained in detail 
later* Now, the original problc® of finding a path from A to B 
is decooposed into two sul^roblems « (1) that of finding a path 
from A to C^ and (ii) that of one from to B, solutions of 
which Tnhen put together yield a solution to the original problem* 
ihese two sut^roblems ere now solved s^arately* again using the 
same procedure as was used for the original problem* 3he original 
straight line joining A and B is recursively modified to ultimately 
yield a path coag)osed of straight line segments, not Intersecting 
any of the obstaclest 

Two of the steps in the algorithm, aaaaely (i) that of 
searching th3rough the line pq (Fig, 2*10) for the points Pg 
and thffli the points and Cg. (ii) that of choosing between 
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and Cg, for deoonpoalng the path, have beai desonhed below 
in detail* 

(\) In Pig, 2,11, let A, B, a, b, c and ^ be ^diat they were 
in Fig, 2,10, To locate the points and Pg on pq, the points 
of intersection of pq with each of the obstacles are first 
determined. Let the distances of the corresponding points of 
obstacle 0^ from b be t^^^ and t^, udiere tj^ < t^, mich that the 
distance is negative if the point lies between p and b and 
positive, if it lies between b and q. 

Let the point Hbl in some obstacle Oj^. (0^^ in Fig, 
2,11) and let the corresponding points of interstsction with 
pq be at distances t^ and t|/ from b, let T» and T^ be the 
required distances of and Pg from b. We first set \ 
and T« a t^. Now let t, and t, be the points of intersection 
of some obstacle other than Oj^ with pq. The values of 
and Tg are now tj^jdated depending on the values of tj|^ and t^. 

Here there are several possibilities. 

Co) \ \ and < ^ 2 * 

* = » » # H 

tr T, tc^ b Ta 

In this case T« is wdated to t, l)y setting Tn « t, and 
Tg is left as it is. 

( b) < % < *2 ^ ^ ^2* 



4-6 
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Here we set Ig = and is left as it Is. 

,( C ) < T^. 

H if <i 

ti T» b -Tf, L ^ 

f ^ 

Here we set « tj|^ and Tg “ %• 

For all other oases, and Tg are left as they are* Ihis is 
carried out for all i » 1, 0KtB4, x k, where 0IiUI4 is the number 
of obstacles* 5he values of and I’g obtained finally give the 
distances of and Pg resg? actively from b* 

Having obtained Pj^ and Pg, they are now offset to and 
Cg as follows* If, idiile comparing t^^, t^ with ^’2* ® situa- 
tion arises where t£ < then 

it M 1 ^ f 

t, be' b \ 

the gap dtji^ « *• t;^ t.s recorded, Slmilarlyi if tj^ > Tg, 

then, 

< < f K ^ 

Tj b Tj ti- be' 

again the gep dt^ •• % *** ^2 recorded# Let 41^ and ntg be 
the smallest of the dt^ s and dtj^ 's, re^ectively# the distances 
of the points and Cg from b are now found as » f ^ 

““ ”1.2 " *2 ♦ It. thar. la a. olalwl. tor 

which, the first of the above two conditions is satisfied» thm^ 
mm multiple of the margin is chosen aibitrerily as the offset, 
and becomes ihere is en integer ithich has been 

chosisi arbitrerily di^witding on the problem, and is the rneri^# 



Similarly, if the second condition is not satisfied for aiay 
obstacle, then found as ” ^2 

(ii) Having obtained the two intermediate points and Cg, 
for the path from A to B (Fig, 2,10), the next sti^ is to make 
a choice, not necessarily final, between and C^, 

The procedure adopted for this is the A* algorithm [9] • 

The digraph needed for the A* algorithm to be ©iployed is built 
as follows* 

Points A and B alongwith every intermediate point obtained 
during the recursion process form the nodes of the gr^h, as 
shown in Figures 2,12 and 2,13. An edge exists betweaa any 
two nodes, correaponding to points if and only if the 

path from to Ng has come out to be the line segment at 

some stage in the recursion process. This will happen only 
when the line s^ent “SJUJ does not intersect any obstacle. 

The graph for the situation in Fig, 2,12 would be as in Fig,2.13* 

A* is now used to look for the optimum path from A to B 
in this weighted digraph iidiere the weight for m edge is 

the length of the segment joining the points corresponding to 
nodes 

This graph can be pruned, before the search is carried 
out, so that unnecessary search for a path through points, for 
which it is kmm that a path is not feasible, can be avoided. 

The conditions wsAtr which a node will be Weted irm the graph 
are as follows. 
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(i) %en the point corresponding to the node is beyond the 
limits of the worki^ace of the robot, the path through that 
point obviously becomes infeasible. Hence that point is 
rejected, 

(ii) Referring to Fig, 2,14^ let and be the mtenaediate 
points for the path from A to B, If is now chosen to lie 
along the path, for the path from A to C^, we have two more 
intermediate points and Dg, It is easy to see from Fig, 2,14 
that the intermediate point for the path from A to will have 
to lie on the s^e side of pq as A, if unnecessary repetitions 
are to be avoided* In Fig, 2,14, Dg does not satisfy this 
condition and is hence rejected, A similar condition is spplied 
to the intermediate points for the path from C. t» B* In this 
case, the two will have to lie on the same side of line pq as B, 

hikaidse If and Cg are the intermediate points for the 
path from A to B, and Cg ai^ first checked for the above two 
conditions. Three possibilities could arise here - (i) both 
and Cg are infeasible points, Cii) caaly one of th«a is infeasible 

(iii) both are feasible. 

Case Ci) ii^lies that, there does not eslst a feasible 
path from A to B, Case (ii) implies that a feasible path from 
A to S will haipe to pass through the Intermediate point ihiish 
is feasible. In oast (iii), the path from A to B could either 
pass through or through Cg, If is chosen to lie on the 

30 ath, then we need to look for a path from A to B, passing through 
in the digreph for the problem* 
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The algorithm A* associates a heuristic fimction h(C) 
with every node C of the gregph. If we are looking for a path 
frcKtt A to B, then hCc) is such that it is a lo\^er hound on the 
actual path length from A to B through C, For a heuristic function 
h(C) chosen in this fashion* A* assures an optimal path from A 
to B if one exists* The lower hound on the path length between 
any two nodes is obviously given hF the length of the straight 
line ^joining the points corresponding to the two nodes, Heoice* 
the lower bound on the path from A to B, through C istl^lt-l^SS|» 

From Fig, 2,14, we have, 


. lfr-w| 
+ It-gU I 




where and are the distances of respectively from 

b. This suggests that can itself be used as the heuristic 
j^mction associated with the node coxrei^onding to the poiht C^, 

If and Cg are the intermediate points obtained, and 
a choice is to be made between the two, then, the nodes are 
ordered first based on the vsdues of hCC^^) and h(Cg), in the 
increasing order of the values of these functions* After this, 
the node with the lowest ''h’ is chosen as the intermediate point. 
If the path through this turns out to be infeasible, then the next 
node in the order is tried out. If this also becomes infeasible, 
then it means, a path from A to B does not exist* 

The sdlgorithm, as given above, will definitely find a path 
if one extsts, but the path obtained in some cases, may not be the 
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best possible path between the two end points, A typical 
situation where this could occur is shown in Fig, 2*15a* 

In Fig, 2,15a, 0 is the e 3 «panded obstacle and A and B 
are the initial and final points respectively* path obtained 
using the algorithm as given above is 

easily seen from Fig, 2,15a that Is obviously a better 

path than what has been found. To avoid such inefficiencies in 
the path obtained, the part of the path generated at every stage 
of the recursion process is refined. The refineaent is done as 
follows. 


Let the path obtained between two points and at 
some stage in the recursion bei as staowi schematically in 
Pig. 2.15b. 


The intermediate points are generated in 

that order. Now paths between pairs of vertices are checked for 
intersections with the obstacles^ ^ese pairs are chosen in the 
order given below. 


*A* *A» ^A* Vi* Vi» 

D.I., D,G., F.B., F.I., F.G,, 

i i’ i 1’ i i' 1 i’ i 1' i i' 


1? tjr 

Vi* 


Vi* 


Let the first pair of points between %diich a pathf not intersecting 
any of tdie obstacles esdsts^ be sayJ|Gj_* Then the path from to 
Bj is ssodified to XG.lfB, and the nodes between X and Cl^ are 
deleted. If this does not h&ppm for any of the pairs i then the 
path is left as it is* 




Fig*2-l5a A by|>‘cat Case of 



Flj.a'lSb 


Refitiement of (safh 
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path obtained at every stage of the recursion is 
refined in the manner outlined above. The final path obtained 
after this would be the best possible path betiireon the two «id 
points. 

This in effect* solves the problem of finding the path 
for a circular ob^Ject to be moved from one point to aiKJther, 
in a plane, through any arbitrary arrangement of polygonal 
obstacles. 

2.3.2 Polygonal Ob.iect 

Here we consider the problem of moving a polygonal object 
which has rotational freedom as well through a masse of polygonal 
obstacles from a specified starting position to a specified end 
position. Here also, we use the same repres^tational scheme as 
described in Section 2.2 along with Fig. 2 * 3 * The idea is essen^ 
tially the same as the one outlined above for circular objects, 
esceept that the problem becomes a little more complicated owing 
to another de|iree of freedom i.e. rotation, for the object# Here 
too, the object is first reduced to a point, ^dJich coiiwsides with 
its reference point end the obstacles are suitshly estpanded* The 
^velope that is obtained idiile ea^anding the obstacle, will 
obviously depend on the orientation of the ob;|ect* Hence we 
end with the grown obstacles being embedded in a >*31 
(estra dimension for the xntatioii). These grown obstacles ere 
not poXyhedra, but have curved surfaces, idiioh makes the taid;; of 
dealing with them even more tougher* 
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^he object orientation (aa defined in Fi^* 2# 3) laay be 
subjected to a restriction that it can lie only between 
^max* range is divided into N e<|ual parts and the 

envelopes for each of these N+i discrete orientations of the 
object, are created around every obstacle# 5he allowable 
orientations are given by, 

** ^min ’** ^ ^ ) , )\ = 0*1, 

^he algorithm for generating these envelopes is given below* 

Referring to Fig, '2*16, let be the obstacle 

and object at some fixed orientation* Let R and R, 

be the reference points of the obstacle and the object rei^ec- 

tively. Now a vertex, say of Ihe obstacle is chosen along 

/ 

with some direction of traversal. Let this direction be clockwise 
(Indicated by an arrow in Fig* 2,16) with reject to R* 3he 
object is now placed such that the point R and the object are on 
opposite sides of the line through and Fg without touching 
the obstacle, being the edge starting £mm along the 

direction of traviffirsal. 

Here, lines parallel to V^^Vg are d 3 ?«.wn through each on 
of the vertices of the object# Let the one closest to be 
with the oorreip<«iding vertex of the object being v^m !Ehe 
«ftVSl©pe is mw generated with V|_ eoad V| as the stttrtiiig vertices* 

Having determined the starting points and v^ in the 
obstacle and the object rsa^mtimXf§ \ ml v^^ are made to 
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oint R * orientation remaining the same. The reference 

P <• of the object at this position becomes one of the 
vertices ^ ^ ^ 

oi the envelope. In Fig* 2.14, this vert^ is 

^^Xerring to Fig. 2.17, let v^v^ be the clockwise edge 
(with rsspect to r) of the object from v^. The angle 

outside the obstacle, is now determined and depending on 
the valu® obtained, the next course of action is decided \xpon^ 

Two difXej.gjj^ possibilities are discussed below. 

(a) AnrX® v^Vj^ (V^)V2 < %i 

this case, the object is translated along the direction 
parallel to till coincides with the vertex of the 

(b) Angle vgv^ (v )v^ > %t 

*^«re the object is translated in the dilution parallel to 
VjVg until Vg coincides with 

one of these is done, the new position of the reference 
point of the object gives the next vertex of the envelope. This 
is carried out repeatedly till the object comes back to Its start- 
ing position. The envelope, which is also a polygon, is obtained 

by ioining the successive positions of the 3?eferejce point of the 
object, 

^“*5 is easily seen tisat the maximimi number of sides of tbt 
envelope win be a-m iliere a and n are the number of sides of 
obstacle and the object respettivtly,' 



^3 
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Envelopes for each orieatation 0^ of the object, /represented 
by plates of thickness e^ual to the constant rotation Interval 
chosen - 0 ^^^ )/N and shaped like the envelopes at the 

corresponding 0^’ b. These plates are then stacked oneon top of 
the other, in the order 0^^ s, with the plate corresponding to 
^min bottoai ana that cozrresponding to 0^^^ at the top, 

to give approximately the expanded obstacle in >*D, This is 
done in turn for all the obstacles in the workspace. These plates 
forming the equivalent 3-D obstacle a3?e usually refsgrred to as 
slices 3 , 

The problem of finding the path from one position to 
another, on a plane, for a polygonal object, such that it avoids 
all the polygonal obstacles on the way, now gets reduced to that 
of finding the path of the reference point of the object, f 3 ?om 
one point to another, in 3D-space through the maze of 3D obstacles# 

Let the initial and final positions of the object be given 
by 0^) and (x^ y^, 0^)# As was done in Sec* 2*3*1, the 

search is started by first joiaiog the initial and final points 
by a straight line and then checking for intersections with obstacles 
on the way* For ai^ glv«a obstacle, this is done as follows# 

For any point (x, y# 0) o® 3.1ne Cx^, y^^, 0^) 

and 0j[^ have. 
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^ *** y ^ y ^ 




X ~ XJL + (Xj - ( 5^ ) 

0 ^ 04 

y - yi (y, - y^ ( ) 


(2*9a) 

(2.9b) 


0 is now varied from 0^ to 0^ in small steps# For each 
such 0, the corresponding x and y are calculated from eqns# (2# 9a) 
and (2.9b), The slice nearest to this value of 0 is given by an 
integer ;}, where ;J is the nearest integer to - 0 ^^y^ )/n03 with 
0 n,iy ,)/H. A Check is then made to see if the point 
(x, y) lies inside the ;}-»th slice. The first value of 0 (starting 
from 0j^) for which this happens gives the point where Idie line 
Joining y^* 0^^) to (a^, y^# 0^) Wters’ the obstacle# 
Similarly t the last value of 0 for vhich this happens would be 
the point n^ere the line ^extts^ from the obstacle# bet these 
two values of 0 be 0^ and 0^, with their correaponding x» y 
coordinates being (x^, y^^) (x^, y^) respectively. Then the 

points of intersection of the lint with the obstacle are 


y®* <*.5 y®* »‘c^‘ 

A cheek of this Sort is made for intersections of the 
line with each one of the obstacles. If it does not intersect 
any of the obstacles » then a palh has been found* Otherwise 
let the points of .intersection with the obstacle which is 
encountered first (starting fro* (a^i ^ Tat 0*) 
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and (Xj,, y^, ®^*%>oint of the 

segment joining these two points, How a plane (p^) at 
Yb* 9^13) normal to the line joining (x^, 0^) and 

^ drawn* The intermediate points are now located 

as follows. 

Let the vector joining (x^, y^, 0^) to (s^, y^, 0^) be 

" ^if ^ ^if ^ * ^if ^ ^if * ^f ** Yif * Yf • ^i 

a“d0if « 0 j - 0^. Consider a unit vector lying on the plane 

Pj^, parallel to the x - y plane at b (see Fig* 2 * 1 & 0 , This 

— A «, A 

vector would be of the form x i ■♦• y j udiere, 

X n -Z^ and y *« vtoere t « [x|^ -f 

because (x i 4. y j). (xj_^ i ^ Y^f ' 3 " •** 0 ^^ h) e 0 , Let these 
values of x and y be denoted by 3^ and y^ rei^^tively* Let the 
unit vector be called $ » + y^ S'* Consider also the 

vector ^ nonaal to both ^ and Jut, i*e, 

f « ^^xff 

** ^*0 ^ y0 ^ ^*if ^ ^if ^ 

•* Y0 4 >if i 0if Yif • y0 

Let the normalised form of this vector he Yj^ ** ^ ^ ’*’ Yn ^ 

%ese vectors are shown in Fig, 2 * 18 a. Now, a line parallel 

to the vector'^ is drawn at b and the points on this line, that 
have one of l^e allowahle angles of rotation 0^. sm marked# If 
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for some such point, the corresponding angle is 0^ then the 
distance of this point from h would be t^ = (0^ - 0^)/?^* 
The x,y coordinates for this point (b^) would then be 

V = \ ' yb + V yn- 


Now a plane (E^) parallel to the x-y plane is drawn at 0 = 0^. 

The intersections of this plane with each one of the obstacles 
would be nothing but the slices of those obstacles at 0 = 

Next, a line (p^ q^) is drawn at b , on the plane P^. A 
search for points and is now made, along the line 

^Axi *^-Ui '^^^y explained in Fig. 2.11. The point 

b in Fig, 2,11 corresponds to b^ here, the line pq to 
and the obstacles 0^, 0^, 0^ and 0^ to the slices of these 
obstacles at 0 = 0^, The points and obtained from Fig. 2. 11, 
correspond to the and in this case. 


This is repeated with M different lines p^ q^, i = l,Mf 

symmetrically placed on the plane about the point as 

shown in Fig, 2.18b. Two points ^;yj(2i-l) ^A(2i) 

> 

obtained for each one of these lines. All the s obtained 

constitute the possible intermediate points on the plane Pa- 

^he intermediate points for all the planes I^, -AA.- 1,N where 

N is the number intervals into which the rotation range 0jjjj^jj“ 

0 . has been divided. This way we get M possible inter- 
’^min 

mediate points, M on each plane E^, 

Now, one of these MN points is to be chosen as the inter- 
mediate point. This choice is again based on the A* algorithm, 
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LoccxttOTi of <n\ tht* |^iS’PK^ f^* 
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as it was in the case of a circular object. The heuristic 
ordering function used here is again the distance of the 
intermediate points from the point b . The distance is cal- 
culated as follows. Let the distance of a point C . from the 
corresponding b^ be t^. The distance of b^ from b is V. 

The former distance is along and the latter is along the 
plane P^. Since is normal to P^, the distance t^^ of 

from b would be given by, 

These t^' s serve the same role as did the t^^ and t^^ 

Fig. 2.14. Here too, some of s can be discarded based on 
conditions similar to those mentioned in Section 2.2. .. The 
only difference would be that in Fig. 2,14, the line pq would 
now be replaced by the plane P^^. The rest of the search is 
exactly the same as the recursive search described in Sec. 2. 2. .. 
Here too, the psfeith is refined at every stage, to finally yield 
the optimum path. 

The algorithm described above for find path with rotation 
will work as such only for objects vdiich are convex. This poses 
no serious limitations however, since any non-convex polygon 
can be broken up into convex sub-polygons. Let the number of 
convex sub-polygons into which the object is broken up be N^. 
Envelopes are generated around an obstacle with each one of 
these convex polygons. These envelopes are now treated 
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as separate obstacles* So> if tJie ntunber of obstacles in 
the original problem were OKUM, we end up \ii-th x ONUM 
obstacles, all of which have been formed with convex polygonal 
objects* The rest of the algorithm is the same as it was for 
convex polygonal objects. 

The implementations of these algorithms are discussed 
in the following section. 



CHAPTER III 


IMPiaCEKTATION OF THE ALGORITHM 

TMs section descrltjes briefly | the iaplaacsatation 
details of the algorithm based on the configuration ^ace 
approach. The first problem that is to be handled during 
in^jlementation is, the efficient representation of a polygon. 

Here the geometrical shs^e of the polygon itself suggests the 
^propriate data structure i.e* a circular linked list# Each 
node of the list is chosen to represent a vertex of the 
polygon. 

Referring to Fig, 3,1, a reference point R Is first 
chosen for evesy polygon. This choice ©an be az^itrary, except 
that the point must be ndthin the polygon, 

A local frame of referffiice x^- y^ is fixed to the 
ob^Ject, with its origin at the reference point. The position of 
the object can now be coispletely apecified bF giving tdie yg) 
coordinates (with r^spmt to the global £rme x«y) of the referee 
point and the orientation 0 of the fwe with respect to 

the global x**axis. The polygon is now r«pres@3.ted t>y a header, 
as in Fig, 3.2, with three fi^ds, one each for x^^.y^^and 0, Here 
by reading the values of these three fi^ds, the posltioa of the 
polygon will be ©oopletely known. 
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Now the vertices of tiie polygon are described with 
respect to the local frame » as x, y coordinates* A node is 
now created for every vertex of the polygon, with each node 
having two fields, one each of x and y, All these are linked 
in both clockwise and the counter-clockwise directions around 
the reference point, in the sme order as the vertices. The 
header intum points to some node in the list. 

With this representation, the coordinates in the global 
frame of any vertex of the polygon are deftermined as follows* 

Let the header read (xj^* y^^, 0). Therefore, as in Fig. 3.3, 
the coordinates of the reference point R of the polygon is at 
<Xj^, yj^) with respect to the global frame 6 and the local frame 
is tilted at an angle 0 to 6, Now the x and y fi€0.ds of the 
node corresponding to the vertex is read. Let these be x and y 
respectively. So m have, 

r m J y? * and Tan •) « y/x 

or ^ m TanT^ (y/x). 

be 

The global coordinates of the vertex can now/detereined as 

Xg « •*“ ** (0 ♦ ^ ) 

and yg « |E|^ + r sin <0 ♦ ) itoere (Xg, jg) are 
the coordinates, in the global fraae, of the vertex. 
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The object and the obstacles are a ll represebted as 
above. The headers of the all the obstacles together, are 
stored in an array. 

In the case of findpath with rotation, the slice at 
every allowable 0 ^ as defined in Sec, 2,3.2, of the 3I>-obstacle, 
is a polygon. Since the maximum number of vertices that any of 
these slices can have is the sum of the number of vertices of 
the object and the 2l>-obstacle, a circular list of length eq.ual 
to the sum of these two is created. The x, y fields of these 
nodes are now made one-dimensional arrays of siae N, where H is 
the number of slices. So that the (x(^), y(-^)) '3 of all the 
nodes taken together would represent the slice at 0^ , 

The next problem to be dealt with is that of cheeking 
if a given line intersects an obstacle, and if it «^es then to 
determine the points of intersection. Considering only the 2-D 
case i.e., the case discussed in Sec. 2.2. , let the given line 
be from (x,, y,) to (x«, y«)* The parameffcric agnation of the 
line passing through these two is, 

aid, y * y^ ♦ t Cyg •• y*|^)* 

Now an edge of the obstacle is chosen, lat the coordinates 

of the vertices % «ad be Ca^, y^^) and (x^, y^) respectively. 

The parametric e^tion for this edge is, 

and, y » 
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So» for the point of intersection of the edge and the line» 
we have, 

+ tCXg - X^) e \ \ 

and + tCyg - "* 71 % ^7;^ ^ 7^) 

These two equations are now solved for t and t^^, 3iie inter- 
section of these two lines will matter only if the point of 
intersection lies within both the segments, or in other words, 
both t and t^^ must lie between 0 and 1, 

i,e, 0 < t < 1 and 0 < t^^ < 1. 

So, t and tj^ are checked forthese conditions* If these are 
satisfied, then the value of t is recorded, otherwise it is 
ignored. This is canrled out for every edge of the obstacle. 

If none of the edges of the obstacle intersect the line joining 
(x^, y^^) and (xg, yg) sub^Ject to the conditions gtpeoified, then 
obviously the line does not cut the obstacle# If there are 
edges ^ch intersect this line, then the two extreme values 
(lowest and highest) of t obtained in the process are noted as 
t md It follows that the points of intersection correspond- 
l,»g to these two values of t are (sEgjt 7^) ^b^ 

*8 - + t, (Xj - 3^). Ya “ Yi * Cyg “ yi) 

. *i + tb <*2 " *1^' “ yi * *b ^3^2 " 

For the probCte® involvli^ rotation of the object, checkii^ 
for intersections of a line joining (s^# y^t 0^) to (x^i y^t 0f)§ 

utiSi npy ©jf tsh# An 
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is based on checking if scaae point (x^ 0^ on this line is 

inside a polygon (slice of the obstacle at ^ or not, !Ehis 
is done by first joining the reference point of the polygon to be 
given point by a straight line. This line is now checked for 
intersections with the edges of the polygon. If there aj^ no 
such intersections, then the point is inside the polygon, 
othearwise, it is outside* 


The A"* algorithm is implemented maintaining an 
ordered list of valid intermediate points for every stage of 
the recursion. This could be an ordlnaxy linked list* 



Each nod# of the list represents ene intermediate point 
and contains values of the x!>*CQordinate, y-coordinate, orienta* 
tion and the value of the ordering function of the intermediate 
point* First of these nodes is chosen for further palSa search* 

If the path is found to be infeasible, then this node is deleted 
and the path search tried out with the next node in the list* 

This is repeated until a successful path is found* At this, the 
values of the x, y, and 0 fields of the successful node ssjre 
recorded and the idaole list is disposed off* If the list becomes 
empty before a successful path has been found, then it means that 
a path from 0^) to (x^, 0^) does not eadst. 
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Tho rest of the in^leiaentation involves the use of a 
psrocedure which would call itself recursively to detenaiBe the 
path. Every level of recursion* contains an ordered list of 
intemediate points as es^lained aboveg and the list is dii^osed 
off while popping out of this level* A path list is also main- 
tained* so that \dienever a strai^t line pai^ to a new pedLnt 
is found from the point which la the last in the list, the new 
point gets appended to this list. The first element of 1diis list 
is obviously the starting point and the search ehds when the 
destination point gets ^pended to this list. Before popping 
out of every recursion level, the path is refined in the manner 
discussed in Sec, 2,2# , The path list generated, is used for 
doing this* 

This i 05 >le©entation was dcaie entirely in Pascal because 
of the complicated data structures that were necessary* 

The actual program codes* in^lesentlng the pcsialt^ function 
algorithm and the configuration space algoritha are given in 
Appendix A and Appendix B rei^pectively, A 

•k^itoii «f tli» lOitrita for tSit 

Ajpace sppmmchf la giWKt In Appet id iii St C*,) 



CHAPTER IV 


RESULTS AND DISCUSSIONS 

lispleraentation of the algorithms pnjposed was done 
on a ND-560 supermini computer and the graphic displsg?' was got 
on Tektroni»-4l09 terminals* 5he results obtained are presented 
below* 

4.1 Penalty Funotion Approach 

4.1.1 Circular Object 

The solution obtained* for a sample probl^ is a^own in 
Fig, 4,1 alongwith the obstacles. The plots of the angles 

i « IpU with respect to the path length is shown in Fig,4,2(a) 
through 4.2Cd), The lengths of the links ®ftd as 

shown in Fig, 2,4 areSio «^/o5o,>^-oand55--oi>»r#spectively, The limits 
on the ;5olnt angles ©g, ©^ and ©^ arei 


®1 max 

m 


f 

^1 min 

*» — II£i'6 

(ii) ©2 uajj 

m 

iiA-6‘ 

» 

^2 »ain 

m -)o-0* 

Ciii) ©2^ 

m 

106-5* 


^3 min 

«, - 106-5* 

Civ) ©4 

m 

119-7* 

i 

atih 

m ^II,£,-6 


The cpu tijie tak«a in this ease is 4*9 
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Fig* 4*3 shows a case where the algor’ithm fallsto work* 

This is because of a local minimiia that is present at the pls*^® 
utoeie the ob^ject can be seen to have got stuck in Fig* 4*3* 

Since a minimtan has been reached, the object does not proceeii 
any further* 

4,1,2 Polygonal Object 

A solution obtained in this case is shown in Fig, 4*4. 

The plots of the joint angles ©j., i « 1,5 in th3.s case are 
shown in Figures 4*5 (a) through 4,5 (e)* The limits on 9^ 

S max ” -iiA-s*** The cpu time taken in 

this case is 4 8*6 see. 

Fig. 4*6 shows a case, vher^ the algorithm fails* The 
reason is again the presence of local minima. It observed 
1:^ studying several cases, that the proposed algorithm worfe® 
well even if the obstacles are closely cluttered, though the 
problem of local minima still r«Bains. Hence, the above algcritto 
can be combined vezy profitably with global search technicinei 
where some intermediate targets could be g€Si€a?ated* 

4*2 Configuration Space Anoroadh t 

The leigorithm proposed under this approachp is applic®^^* 
oiily for cartesian robots* The aim here is only to obtain a 
path for the object* that avoids collisions with ai^ of the 
obstacles* 
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4*2*1 Giaaeratton of Envelopes « 

Figures 4*7 and 4*8 show tiie envelopes gaaerated around 
a polyhedral obstacle for two different orientations of a 
rectangular ob^Ject* 3:he envelopes have been shot^?n by dotted 
lines, Th& algorithm for generating these envelopes has been 
discussed in Sec* 2*3* 

4,2*2 Gix*cular Qb.iects i 

The path obtained for the case in Fig* 4*3» using this 
algorithm is shown in Pig, 4*3, The path shown is the final 
path idalch is obtained af-ter refinements as discussed in Sec, 2* 2* 

She c|Hi time for this case is i l 5 e,c. 

^•2,3 Polygonal Qb.iectst 

Figures 4,10 and 4,11 show the final paths generated by 
this algorithm for two cases iidiere the one in Fig* 4*10 corres- 
ponds to the one in Fig, 4,6* As can be seen from Fig, 4*6f the 
penalty function method failed in this case* This particular 
problem has also been solved by Lozano-Peres and R*A, Brooks [63 
ttie elution for which they have shown in their pe^ia*. Their 
running time was of the order of 10 minutes of wall clock time 
on a single user HIT lisp machine* According to them, this 
problem is one of the toughest fin<%>ath problems solved %y any 
program# The running time of this problem in our case is 22.8 sec. 
on time shared machine with 7 terminals* 
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CHAPTER V 


CONCI.USIONS 


The two new algoril^iias for find path problem have been 
proposed in this work* The first algorithm is based on Penalty 
Function i^proach * The proposed method of working in ^oint 
coordinates directly is shown to work well and offers the advan- 
tage of taking care of the limitations on the ^oint rotations 
simultaneously idiile finding the collision free path. Moreover, 
since there are no spproxlmations on the shapes of object or 
obstacles , the obtained path needs no further checks or modifi- 
cation. The algorithm is not suitable in dealing v/ith Ihe cases, 
nhere the object has to circumvent a relatively Icaig obstacle. 
Otherwise, ittie proposed algorithm is successful even if the work 
space is thickly populated with obstacles. 

The second algoritlm is proposed along the lines of Free 
%)ace i^proach , The suggested method of concents^tihg on the 
free i^ace only around the obstacles is found to be successful 
in all cases tried, even with the long obstacles, where the first 
algorithm failed. With the second algorithm, the objective was 
only to find a collision free path. In this woxk, no attempt has 
been made to solve the inverse kinwatic problem while using the 
second algorithm. Ihis algorithm, though, is very useful for 
mobile jBubonomous robots vhich ga-ttier the workspace iafomation 
through vision systems. 
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flPPEMDIX fl 


PROGRAM IM>:‘lEMEN ! IM6 THt P-NAuiy FUNCTICN A^GORITH''! 
program avo i ooDS'cacie ( input , out put ) ; 

laDel 

1 00 ; 
c orist 

convert conversion factor froivi radians to degrees ^ 

n diiYiensio}tai i ty of the reduced joint vector 
lYiincb , lYiaxct — aroit rary constants 

coYwert~57- 29577951 ; 
n=3 ; gr c^=68. 2Ej.b7 ;; nst eD=5 ; 
lYf inc.'t^l . 0t--4; jYiaxct = l . Oe-i-S; 
pi = 3. : ^1.15927 - 

type 

vector-ar ray Ci, „ viJ of real : 
obst set *=??se t of l-.iO; 

pointer — nooe representing a vertex of a polygon> 

pointer-’ node; 
node=r(eco ra 

X, y, theta, I s real ; 
next, prevs pointer 

evid ; 

pointerp — nodes representing the centers of the hyootneticai inscriping 
Cl rcles 

po int erp= nodep ; 
nodep- reco rd 

r, t beta, rao s reaJ ; 
next 5 po intero 

end ; 

X «-**-«^** X'^*****^*X-X- Jt*X-*-X'**’X-******^-X***X-*-X‘-X' 

pspolygon — neacer vioce represent ir^g a pseudo-jiolygoni formed toy the cenb^'^''-^ 
of t ne hypothetical inscriPing circles 

^***** x-*'****-5)e-^***x-*^(**^-x-**-X‘’X*’?e*'Jt-* )(-#t-***-x-***^**-x-*****^**-x**‘X-'X-* 
psDO iyg OY’-j— ’ If rarviep* 

1 f r«\iYiep~reco rd 

X, y , t het a s real ; 

Yiext s pointerp 

end ; 

polygon — heacer noae represer^t ing a polygovi 

^^^^^^^*^**>t.^**.j#.^*.y.#^***#***^**********#******************‘H***-i»-********** 

p o 1 y g o n- ' 1 f ram e ; 

I f r ame — r e c o r d 

X, y, theta s real ; 
next 2 poi Viter 

end ; 

var 

grao, jc, jcf, jcl, ssvector; 

X lnYi, ul im, fact sarray u0..4J of real; 

pni, xf, yf, fDhi, xi, yi, iphi, 10, 1 1, 12, i3,ste3, gstep, yO, ^lope, factor, eC^ 
ct, o2, a, D, c, xi, yl, rot fact 1, rot fact 2, rotfactS, slperr, oisterr, cj ist 1 , dfsU 
si, xg, yg, rotg , approach s real; 

1 , j, nvert, onumi, ox, oy, p, couuit, npseuds: integer; 
obstaclesaxrray Cl.. 103 of polygon; 

vertex 1 ! pointer ; object ‘.polygon; psobject ; p spolygon; 



c: 1 e s ODstsei: ; 

ciecH s 'jdo 1 ean ; 
a f’l : T:eKt ; 

i^inclnne ( : 3 wa<*v) g raph tcs i oas 

’oerinfo’ cotYiDutes tne glooal coora inatesMx, y ) ’ of tJ'ie vertex ’vertex’ 
whjcn IS iv-j tlie polygon ’object’ 

■¥r^-¥r Jt** X* H* >< )<-*4e**K'*i^*****’?t-^t-***^i'**->f*-i< ^ 

p rocec 1 1 re get inf o ( o bject s po i ygon ; ve rtex 2 po int er ; var x, y 2 real ) ; 
var ^ 

xl, yi, x 2 , y 2 , r, the-ca s real 5 
begin 

xl 5==object* . X ;yl s-objecc***. y; 
b neta 5 =ve rt ex v b het a*<“ 0 bject" .tneta; 
x2 2 --vertex"' ^ x ; y2 £ =ve r^ex ’ . y ; 
r 5=!sqrt (x2*x2“{-y2*y2) ; 

X s-xl + r-i^cos (t beta; ; 
y i ==y 1 - 1 - r*s 1 n ( b het a ) 

end « 

’Psgetinfo’ coiYiputes tne global coordinates ’ (x,y>’ of tne centre of tn<t 
Hypothetical c.i rcie given by ’vertex’ of tne poiygovY ’object’ -It also 
gives the radius ’rad’ of the circie 

■^^•*^*•^^■}^^**f-•H'-J<■ 

procedare psget iYif o (object £ pspolygo'n •; vertex s poiviterp; var x, y, rad s reai ) 
var 

xl, yl, x2, y 2, r, -checa £ recx J ; 
beg 1 n 

xi5-DOject •. x;ryj s=odjer,t -y; 
b ne ba 2 -vertex ' . t het a-i-o bject *- t ne ba ; 
r 2 -verb ex ’ - r; 
rad £ ---ve rtex**'- rao ; 

X £ - X 1 + r*c o s «» t het a ; ; 
y : -= 5 y i H- r-^s i Yi ( t het a ) 

evid ; 

**■)#• ***’Jf'*-}f ******'J^‘ X- * * •«• * •»» 

’LiYietopt’ IS tne distance of tne poivYt ’ (xc, yc)’ froiYi the line joiniYig 
the points ’ (x^^,ya)’ ana ’ (xb, yD) " - Linet opt is assigned an aroitrariiy Ovfj 
value if pe r pend icuia r froivi tne pt- to the line does Yiot fail within the 
secjiYien t 

f unct 1 ovi 1 1 net opt \ ><a, ya, xfo, yb, xc, yc 2 real ) s real ; 
var 

1, d 1 , d2, cosi , cos 2 , a, □ 2 real ; 
beg in 

lz^( xa-xb) (xa-xb; H (ya-yb) * (ya-ybi ; 
d 1 2 - (xa-xc) *(xa-xc)+ (ya-yc) •)€• <ya-yc) ; 
d2 2 - <xb*~xc; * ( xo- xc) + <y b-yc ) * (yo-yc) ; 

if (abs <dl ) <iyii Yict ) or (abs (d2) <iYfinct ) tnen 1 met opt s-0- O 
else 
beg 1 'n 

cos. I £-dl'+-l'~d2;cas2Si=a2-+'l-^dl * 
a 2 =2. 0*sq rb l-^-l ) ; □ s =2- 0*sq rt <d2*i ) ; 

if (aos ( 1- O-cosl /a; <IY^lY^ct ) or (aos ( 1 - 0 -rcosl /a; (lYiinct .« \h 
1 inetopt 2 = 0- 0 
else 
beg i n 

if (aos <cosi/a) <iYdnct ) tneyi 1 inet oot 2 =sq rt (ai ) 
else if (abs<cos 2 /b) (lYiirict) then 1 inetopt 2 =sqrt< 
else 



iYiove(Kl, yl) ; 
reoe.'at 

vert g?x I =-ve rtex . viext • 
get 1 nfo ( odj , vertex, x, y) ; 

X i : --t rune ( (x- 100. 0) *g ra ) ; y 1 : =t rune ( (y-lOO. 0) i^g ra; ; 
if (xl<o) then xl!=0;if (yi <0) then yl!=0; 

If (xi>4090> tnen KjJ=4090;if (yi>3080) tnen yli=-30aO; 
d raw <xl , yl ) 

until (vertex=oDj- . next ) 

ena ; ^ 

JointsDace inverse t ransf o rmat i on from cartesian coo rd. M x, y, phi ) ’ to 
tne coo)'CJu gaven by tne vector " ic’ 

crocedure J oi ntspace (x, y, phi : real ; va r jcsvector); 

var 

d, d 1 , d c.', a, D, c, al , a2, a3, t hetaO, t het al, t net aJl, t het a3 s rea 1 ; 
begin 

d:~x*x+y*y; 

dl :=13-10+y0;d2s«al*dl ; 

tneta2 !=a rccos ( <d+dr'-12!f-12-l l*i 1 ) / (2. 0-^1 l*i2J ) ; 
a 5 = i2'^cos (t het a2) +1 1 ; b !! = 12*-s in ( t ne t a2) ; c i =sq rt ( d ) ; 
al : —a^ a^ D*b; a2 s =c*a ; a3 s =c*c-b* 3 ; 
a !=sqrt <a2*a2-a3*al ) /al ; bs =a2/al ; 
al ! --o-a ; a2 !=&-i-a ; 

if (al(O.U) then t letal !=a rccos (a2^ else t ne^ca 1 s =arccos ( a ; 
1; hieta3! =the'cal-t hBta2+l . 57»';)79&33; 
tnetaO : =a rctan ty/x> * 

jcC U :=tnetaO; jcf23 : =t heral ; jc 1:33 s =pni~jc Cl 3 -*•1 . 57079633 ; 

end ; 

**-*-)f-**-^* K J("***i^**-»'**-********-4 »•**•)(■ i(-i(.*****-.-i<-#**»>-)f-*#+#*^h**** )(. 

Given tne joint vector ’ ooint cartes’ comoutes tne carTssiavi coorc, 
’(x,y, pni)’ and tne joint angles ’theT:a2’ and ’theta3’ 

■*•***■*■**•}*•*■*•« X•*■*■*•»■^^•*•*•***■*■X• )(••*•*■*■)(■•< 

proceoure cartes < po int 5 vect or ; var x, y, pna, theta2, t hetaGs real ; : 
va r 

t net aO , t het a 1 , a i s rea 1 ; 
taeg^i n 

■c het aO ! =point ':i3 ^the-cal : = ppint C23 ; phi :=point C3j ; 
t het a3 s =a rccos ( < 1 l*-sin ( tnetal ) -i3+i0-y0) /12) ; 
t heta2 s =:t hetai-t het a3+l . 57079633 ; 
al !! = 12*sin(thBta3) +1 l*cos (t hetal ) ; 

X s-al *cos (t ne-caO) ;y :=al*sin (t hetaO) 

eno ; 

■S-****-^ ■* + *•« *-K ■»♦■><•** *•■><**■»<•*■)*•*■»*■*•*****#******■)<• ■JHHf-'y-*^-****** 

’ Funval’ is tne value of tne objective function at the point given dy 
tne joint vector ’ooint’ 

**■****■»*■¥■ i^-¥-X-»******-***********-lf-*** **-i(-***‘#***--¥-*-**^-»**-<('#****il-********it*-i(-i(riir 

function fuYival (po int ! vect or ; sum t real ) ireal; 
var 

theta: array CO. . 41 o'^ real; 
a, d, X 1 , y 1 , suMl , sum2: real ; 
i : i riteger ; 

f unct ion dist ancefn ( i-s integer) : real ; 
var 

sum, X, y, r: real ; 
vertex : pointerp; 

’ pdist ancefn’ is the dmin between tne polygons ’object’ and ’obstacle’ 

********->(• ■>>--*t*-}«-****************-<f'**'*^^**''«'*******-*******»*********-»-i^****-^'*'r*')»- 



if ((co^sDO.O) and <cos2;0.0;) tneen 
1 ineb opt :=<^qrt (c2*“'Cos2-)^cos2/ (‘^•^1 ) ) 
elsfc^ 1 met oDt s=jYjaxct 


end 


end 


end ; 

"Max’ cnc.^c^«*> if ’ t ** is ^arge (iVtaxct) or not 

f u nc t 1 0 ) I iy<a X ( t 5 rea l ) J bo o 1 ean ; ^ 

beg J n 

if (adr> \ t -‘jYiaxcr ) COO.O) thevi inax 5 -i; rue 
else ivtax s -fa Ise 

e)id :; 

’ Prccos** coiYiDufces tne ang^e whose cosine is ’cx’ 

-3f.¥-^ }( •X'^f ***X-*-X**** 

function arccos (cx 5 real ) sreal: 
va r 

sx , a 51 rnal ; 
bog -n 

Dt (aos^ (cx-K 0) <iYi:nct; tiien aiccoss-O.O 
e i se- 
lf (aos CcxH“l. O) (lYiinct ; then arccosi-pi 
else 
begin 

sx s ~sq rt ( 3 -cX^cx ) ; 

if <abs <CK ) > lYiinct ) then a s -arct an Csx/cx) 
else a5==pi/2.0; 
if (a<0«^"') then as-a-+*oi; 
arccos 5 =a 

end 


end ; 

proced ure 
var 

X 1 


d rawci rcle (x, y, t ‘ real ) ; 


y J s j ntege V ; 
t netsa, dtneta • real ; 
beg 1 n 

t net a s —0. O : d t het a 5 =0,. 2 ; 
yl s •=t rune ( (y-100. 0) *gra) ; 

X 1 5 =*• t r unc < C x-f- r-1 00. 0 ) *g ra ) ; 

jY{Ove(xl, yl) ; 

repeat 

t hera s ==t het a-i-ct het a ; 

xl 5 =t rune < (x-t-r^cos (tneta) - 100. 0) *Qra) ; 
yl s=:;:t rune ( (y4T*s in (theta ^-100. 0->*g ra) ; 
d raw (xl , yl ) 

unt 1 1 (tneta) -6. 2S3I85<ii ) 
end ;; 

procedure brawpoiygon (ooj s po lygon) ; 
var 

t neta, X, y " real 5 
xi , yl 5 integer; 
vertex s po i nter ; 

Deg 1 VI 

vertex s=obj*"‘*. next ; 

get info (obj , vertex, X, y) ; 

xl!=trunc( <x-100. 0>*gra) ;yl !=trunc( (y-lOU. o> * q ra) ; 
if (xKO) tnen xl!==0;if (yl <0) then yl!*0; 
if (xl>40g0) tnen xl !=4090;if (yl>3080) tnen yli=3uat.); 



i xon uaistancefn (object, obstacle 5 polygon ) .1 real ; 

1 aoe - 

100 " 

va r 

xo, yo, pni i, prii2, x i , y i , x j, yj , 1 1 , 12, ox i , oy i , ox j , oy j , d i , 
04, a, d ist , cs s real ; 

K, J s integer; 

vertex i , ve n:ex2, o vertex L , o vert ex2 s do i nt e r ; 
sobjec b sa r ray 11..2J of polygon; 

Bve? rt ex i{ array Lj«.2Ii of pointer; ^ 

X 1 1 , X J 1 , y . 1 1 , y J 1 , 1 5 a r ray C 1 « . 23 of rea i ; 
cw« array Cl. -2- of Dcoleavn; 
seas boolean; 

proced Lire 3 ocsect o r ( o ot s do lygon ; phi s real : va r vi , vj s do i nz e r 

va r X i , y 1 , X J , y J s rea 1 ) ; 

va r 

1 oni , J pni i real ; 
ve rtex s DO inter ; 
beg 3 tj 

vert ex 2 -=obt • .next; 

ipni 2=vertex . bneta; 

jpm s-v/artex .. prev « t*iel*a; 

(ipHi^phi) and <jphi>p)-n) then 
repeat 

vert ex s =:ve rtex * . vnex b ; 

J oh 1 2 -vertex* -tneta 
Lint 1 3 ( J on 1 > pn 1 ; ; 

VI s -vert ex . prev ; 
vj s =ve V L 0 X ; 

• get 1 nf o \ont, vj, xj, yj ) ; 

get info ( o bt , v i , x i , y i ) 

end ; 

fun^irtion perpaist ui i , dl', 3 sreal> srecXJ. ; 
var 

ros3 , cos2, d* real ; 

Deg in 

cosi :=fjl-+ 1 ~d2;cos2s =a2+i-dl ; 
if (cosl>0«0) and \cos2>0*0> then 
begin 

ds— d2+l-“di ; 

perpoi St s — scj rt <d 2 ~*ci’^d/ C4’^l) ) 

end 

else perpdist s---? - 0 

pna ; 

p ro ced li re c i s b ve rt ( i s i nt eg e r ) ; 
var 

J ; integer ; 

XI, yi, d,dl,a2sreal; 

ver bex 2 po ly iter ; 

peg in 

if ( 1 = 1 ) tnen ji-2 else 
vertex ! =svertex C i3 ; 
if (cwCt.:. > tnen 
beg in 

XI :=xj I Ci3 ;yi 5=yj i Li 3 

end 
else 
beg 1 n 

xi!=xil Ci3 ;yi !=yil 1-3 3 


enc 



d 2 -d 3 <r> fc ; 

if (cwfij) trievi d-'s-d^ elsr* cils-ds", 
repeat 

?^VGrt ex C 3 ] s ve)' I ex • a ist 2 -f3 ; 
if (cwCiJ) bneri dss-c3 2 ei^^e dss=^di; 
if (cw I ; I; ) t nen 
Deg in 

xj j ri3 s-Ki *;yj j Li j i;=y3 

eno 
deg 3 n 

xil r3J5-x3 “yitCiIis-yi 

enc ; 

if (cwfij) tneni vert ex : -ver bsx drev 

(*lsG verrex s=verbex*' « next ; 

get 3 nf D leodject C 3 I! , vertex, x 3 , y 3 • ; 

d 1 5 -= ( X 1 -x 1 1 [ j 1 ; •)«■ ( X 1 -K 1 1 C j ] ) i y 1 -y 1 1 L j D ) y i -y 3 iD* 

d? S - ( X 3 -“X j I •: j D ) -^ ( X 3 -X j 1 r j li ; “H • y X - / J 1 : J J ; •' / 1 y.iiC 

• 1 S“perpd3 st Id 3 , a2, i Cj H ) 
u‘nt 1 1 (d=*-L,0> or (d>dist3 

enc ; 

deg 3 n 

xo s =-oDi:.>T;ac “ e •. x ; yo s -ooLstacl e ' - y : 

1 f ( arjH ixo-H 1 ) > 1 „ 0e-“3,‘ tnen 
beg 3 n 

phi J s-arct an ( (yo- y^ / (xo-x 3 ) ) ; 

if (xo<x-) tnon phi t .*;=oni 1 -J 3, Ih 15927 

else 

if cyo<yl) tnen phil 2 =s:Dhi i-i-€,« 283lSb4 

end 


ei«^e 

3f (yo<ya) tnen pni 1 s -4- 71238899 
else Dhii :=1. 57079633; 
pni2 2='pni 1 ‘+•3, 14i59Z'‘7; 

if (pm 2;6« 2B3JG543 tnen onil' s -ohi-l-G. 2B3lB5a; 
ph3 1 i -pj-Tt 1 -object' « tneta; 

if < pm 1 <0« 0) t nen Dhi 1 s =Dh3 1-^6. 2831654 ; 

If (pni i>6« 2831654) then pH 1 1 s ::==pn i 1~6- 283 1654 ; 

1 ocsert 0 r < ooj ect , pni X - ve rt ex i , ve r fc ex2- x i , y 3 ^ x j , y j ) ; 
i oos^az o r io bst ac le, pn i 2, o ve r t ex 1 , overt ex2, o x i , oy i , o x j j 

1 1 s -vertex 1 . 1 ; 12 s-o vertex J *.1; 
dl5^(X3— oxi)-^(xi*-"oxi) 4-(yi-oyi)*(yi-oyi) ; 
d 1 et 5 -cl 1 ; 

d2 n <x i-oxj ) (x 1 -ox J ) + ( y 1-0 yj ) -^ <y i“oyj » 
if ^d2<d3St) tnen dist2=d2; 

d3““-(xj-oxi)’5t<xj-oxi)-+‘<yj-oyi) ^lyj-oyi / ; 

if ^a3<d3St'* tnen diBts-d3; 

rj4s - (xj-ox J ) (xj-oxj ) -V (yj-oy J > * ’ yj"“oyj > ; 

iF (c4<aiet> tnen dxsts=d4; 

died: 3— sqrt idi st > ; sea 5 -false: 

c : -per poist ^ol, d2, 12) ; 

i-f (cO-l.Oi ana (d<dist) tnevi 


Dec i n 

sea « =t rue ; 

cw C 1 3 3 — t rue : cw ^23 s — f a i se : 
cs5-"C2;aist s-d; 1 123 5 = 12; 

sve rt ex L 1 3 - — ve rt ex 1 ; sve rt ex l2 J • — overt ex2 : 
xjlLi: 5==xx;yjiCi3s^yi5 
XI i C2J s=ox3. :yi 1 C23 J==oyi ; 
xj i e:::3 -yj 1 l 23 5==oyj ; 



<?ot.jecr cn :=object ^soojBct r2j !=obscaclfc 

end ; 

diJ ~,jerr>ci--.t (c\r>, da, I - 
- f ud < > - L . •"/ ) and < c < d i s b ) t hen 
oeg 1)1 

sea « -t )*'ue ; 

cv^ L 1 j s -= r a 1 <:3e ; c w J T-' li s =• t r ii e : 
dss--=d3;dist :-^d,- 1 LIij 

sve rt ey J 3 : =vert ex2 « sve r-cex l22 : --o ve i-'t ex 1 ; 
SGDjeoi: III j s=object ;sobject r2j s= 0 Dstaci% 3 ; 

X3 1 [13 :='<J ;yit 1-13 !=yj; 

X 1 i *_ J J : “ o X 1 “ y 1 1 il 2 3 s = o y i ; 

XJ 1 r:-3 S ^oxj ;/j 1 C22 i-oyj ; 

erIG * 

d 5 =•■= pe?) pc 1 St ( d 1 , d 3, I L j - 
I F ( d <> -“ X « O ) and ( a < (j i s L ) t hen 
beg 3 n 

sea a =t i ue ; 

cwL j 3 :~t rue*;cwL2j -'--falser 
os x -c3 ;r ist 5 =a ;; 1 [21 s 1 ; 

sve )'t e X r -» “I s =o ve r t ex J. ; sve r t e x ; 2 s =*- ve rt ex 2 ; 
sohject Z\ J 2-obs'j;acle:sobject: l2’i * -object ; 

XJ ( r .1 3 s =ox I ;yj I C I J J-oyi «; 

X I 1 [ 2 *} 2 -• X 1 ; y 1 1 [23 2 =:y X r 
XJ L [23 J.‘=-xj “yj } i2“l 2 -y j 
c'nc ; 

a :=5pe)-po-j c,t (^ j 

3 F (o<> -*t«0) and ccuiist) then 
beg i n 

sea s =--c I'ue; 

cwF J 3 ssrfaI<:5e;cwC21 s-b rue: 
os s --cl'^oist 5-a • 1 [21 L ; 

sve I'tex 3 3 3 : =over b ex 2 ; sve '"t ex C , *3 s = ver bex 3 “ 
so bje(?t [ X 3 s=:oDst ac I e : so dj ect [23 s -object ; 
xal[lJ5=s:oKj;yiliil s-oyj 5 
xitC23:-xa;yiX [23 5=yj ; 
xj3 [23 s-Hj -yj 1 C23 s=yj 

enc ; 

if (sea) then 
repeat 

fo) k2=l to 2 00 
be?gin 

c J st ve)'t ( k ) 1 

if (k-i) then j2=2 else js = i; 
xf (cwCkJ) then 
begin 

get info (so bject ilk 3 , svertex C k " * p «'ev, x 3 
y X 1 [ k 3 ) ; 

1 ThI 5~svertex Ck]*'”*. prev . 1; 
d 1 i — sq r < xi 1 Ck3 -xj 1 [ j 3 ) -^-sq r <.y 1 1 C k 3 -yj i [ 
svertex Lk3 sascsvert ex k.. **•„ prev* 

XI t CjJ 5-^xj i i:j3 ;yi 1 [jj i-yjl CjJ 

end 

else 

begin 

get info (sobject Lk3, svertex [kj"’'* next, xj 
yjlCk3 J ; 

1 Ck 3 ;=svertex Ck 3 i ; 

dl 5 =sq r(xjl£k 3 -x 3 13 j 3 ) +so r [yj l C k 3 ~y 1 1 C 
svertrax Ek 3 !=svertex Ck j next ; 



xjl rj3 : = X3 ;yjx Ij3 i!=-yii Cj. 


enc ; ' 

d S"~ :*erpcis:r, (dl, ds, 1 Ck] ) “ 

if (aO-I.O) and id<dtst) bnen dists-d fr--«=e goto 
1 00 


eno 

until Cd=~L.O) or (aOd-isi); 
J 00 s 


pna Ftancef ni - I / dist; 

enc; v 

cc : u a nC's- f r< is ti‘ie dim n Detween tne circie of radius ** rc’ wifh centre 

« oc'.Auec an: * ana bne polygon ’ooject’ 

runcT^ ion cdistancefn ''xc, yc, rc t real ; oDject 5 oolygon/s real ; 

V a r 


^^7 y^-, -xj, yj, vdist , edist , a, d, c, g, cos'" , cos 2 , ea i real : 
vertex, ve r t e »< 1 J po i n t «e r ; 
beg j v’l 

ve ft ex L : = o hject . next; 

net 1 nf o ( ooject , vertex 1 , x i , y i ; ; 

a i =~ < X 3 ~ y c ) ‘^ ( X J -x c ) •+ ( y i —yc ) »> y i -yc' > ; 

vd 1 st f ™a ; ed b 2 = L . Oes-^5 ; ve rt ex s -ve rt ex : ; 

reD6«at 

ve f t ex i “V€3 rtex miexr; 
net i r*< ( ob^, ec I , v(* rt ex, x j , y j > ,* 
b 2 - ( X J - x(' ) , X J -xc ' 4- C yj -yr ; * < vj -yc ? ; 

xf (d^vdist) bhen vdist:=D; 
c 2 - ( X j-'-x 1 > * •' X j~H 1 ) -»- (yj-y X ) * k yj-vi ; : 
cosi s ~a i-c-b ;cos2 2-b-»-c*~a; 
if (cosi>0.. 0) and (rosTOO-O; tnen 
begin 

ea 2 =sa rt > L'~cos 2 'i^cosI / (4-J^c) ) : 
if kea^ecjist) then edists==ea 

end ; 

XI 5 -xj ;y3 :-=yj r,as==:D 
u n b I i k ve rt ex — ve rt ex 1 ) : 
a 5 =• ea 3 «>r- rc ; D : =sar b ( vd *i st ) - rc ; 
if kako) Ihevi cs-l/a eise c5 = ?u/d; 

’f (c;O.0) then ccxstai icef n 2 =-c else caist;ancefvi 2 -l« Oe+5 

enc ; 

Deg i n 

suiYi s=cOi^pdistancefn (ooject, oostacleCil ) ; 

vertex “ -Dsooject - next - 

repeat 

psget xnf o < pso bj€?ct , vert ex, x, y, r > ; 
sums -suiYH-cd istancefn < x, y, r, obstacleC x 3 > ; 
vertex s =:ve rrex • next 
ikntii (vert ex-psobject next ) ; 
distancef n 2 -sum 

end ; 

Deg 3 n 

d5-:O,0; . p, 

for i. :=!:3 to <n -1 ) do d s - qh- ( oo xnt C 1 3 ~jc:f C i 3 ) * ( do x nt i x 1 -jCVt' 
if (suiYi <cxst i ) then ds-dfrotfact:i«-kpoxntL33*-jcfL33)'?s* 

( Doint iI33 “jcf C3>3 ) ; 

cart es (point, x 1 , y 1 , t beta 1143 , t net a il 23 , t heta CJJ ) ; 

ODject xs=:xl •; ODject-*. ys-yl ; 

ob J ect ***** t het a s = po 2 nt l 33 -^po i nt C 1 3 — i * 5 7u796c>v:» ; 
psobject \ X 2 -xl 5 psobject’**. y s-yl ; 



;*:‘Sobject • i- net a s -otDject - t het a ; 

SUlYl 15-0-0“ 

\f (cnecK) then 

for' j 2 ~ 1 to o n uiYi a o 
Deg in 

a 2 --G i«-t ancef n \ 3 ) - 
if va> approach) then 
beg 3 n 

cist oDBt ac ie 2 -cl St o DSC ac 1 e-<- u 1 1 : 

SUfYli 5 — SUiYll+a ^ 

end 

else cL5toDstacIe2=cIstoDstacle- fi J 

end 

else? 

for to onuiYi ao 

3 f ( j. 1V3 cist obstacle) then 
sum 1 s — sufYi i -td 3 st ancef v"t ( 3 ) ; 

'»um L ! ~csuiYil + ( 1 / <x i- j 00) ) + ( J / (yl-1 OO) ) +i / ( . 50-yl ) H-i / ( 100-)CO 
cne-fcaEO: : = po] nt i i: jtneiai j 'J ! = point .2j : 

! -Ou 0 “ 

fo r 3 2 “ 0 t o 4 0 0 
DC'y irj 

a 2 ~ ( t he r r 1 3 - 1 1 i iy . i / fact Lili- 

^nirj2 ; =s«uyi 24 ( l/a)H ( 1/ ( i-a) ) 

end “ 

•^unval ii=d^ ( H“Ci'}^sufYil-Hcj:**suj'ri 2 ) 

enc ; 

’ no TfYh IS the no tiYj ( leny t n) of bne vector ’x’ 

¥¥ f ^i(r¥‘¥ *r¥r Jt- *5^4* 

f LiYic b 1 o n no rivi ( x 2 vec t or): rea i ; 
var 

1 s 3 nteger ; 
su‘Y} s rea 1 ; 
beg in 

suiYj 2 s=«")« 0 ; 

for lS = '5 to n CO su}YiS=-suin+x La3 Jt-x fi J 5 
no riYi f. -sc rt l suiYj ) 

end ; 

’gracient" coiY.prites the gradient '‘yrad’ at tlie point give^-j oy the joint 
vect o r ’ po3 nt ’ 

procedure y rad lent ^po int s vecto r 5 var g rad s vect or - var slope: real) ; 
var 

i s integer - 

f 1, ri', BUiYi, fact s real •; 
begin 

B 1 .UY 1 s -0- 0 ; 

for is — l to (nj-1) do sufYi:=suiYi i-ipjoiYit ii 3-3cf Ji3 ;* (point lij-jcfwJ 
if (suiYi) d ist 2) then fact : -rot fact 2 else f act : =rot f act 3 ; 
for i3=*l bo n do 
bey in 

point Ci 3 :-=• point f il+gstep: 
fi:=funval (00 int , suiyi) ; 
point C ij s- point C i 3 -: 2 * 5 ^osteD “ 
f 2 :-funvcil (point, sum) ; 

if (i<>3> then graa Til : = (f i--f2) / <2*gsteD) 
else grad Ci3 :-fact* (fl-f2) / (2’J^astep) ; 
po i nt 1 1 3 s = po int C 1 j+gstep 



; 

5 o • - no rivi » rj carl ) ; 

r 0 i' 1 *. - ’• i f * d o g r ctci 1 3 1 "• -g racJ i' i 3 /s 1 o De 

ei u; " 

j.'. r • H-'r-CJ » I fJ ‘ : 

' r 

s 1 , y t , pm s rea L ; 

V< V .1“ -V it* **^4-**** )«•**)( *^** 

’oiit’ --output procedure 

^ ***•J^•-^^•* •****#•* * #►•*)► jtif* *** **-Jt ********‘^-< * *■##** 

procedurp out : 
var 

jci , jcd s vect o r ; 

tnetai\ tnetaS, x, y, rot, d rot, a s reaJ ; 

1 , j : integer ; 
beg 1 n 

cartes (jc, xl, y 1, pni, toe tal', t neta3) ; 
object X .1 “xl ; oDject y 5=yi ; 

ODJ ect ’ ^ t heta s =phi+jc 1 1 J "-“t » 57079S JJ ^ 
psot 3 ject X s = x 1 ; Dsobject*"*. y s==y 1 ; 
pso 1 )ject**‘ . tnetas-ooject"*. theta; 

X s~ ( X 1 - 1 00.. 0) *q ra ; y s = (y 1 -1 00. O) *g ra ; 
rot s=object . t 'net a*convert ; 

0 : - (xcj-x ) * ( xg-x ) ♦+• (yg-y ) * Cyg'-'y > ; 

d j'ot 5 - ( ro fc-ro t;g ) * < rot-rot g ) ; 
if ( d > 2SOOO ) ct r < d ro t > - j 000 ) t hen 
begiy) 

drawpol ygon(objecT) ; 
xg i-x ;yg 2-y ; rotg:-rot 

end 


end ; 

)t*** ** ****** *** * ***• ^ ^********-*******’****** **** 

’converce’ — checKO-> for convergence 
********^.,),^^ ** h* ** * **•*•**•* *★**.*****■****•***•***■* 

proced are conve3*'Qe ; 
va r 

si, S.2S v'eal ; 

1 5 integer; 

Deg i.n 

sir -0.0; ^ .. 

for is=l to <n-i^ do sd J = 5 i + ( jc l i 3-jcf f i j J » ( jc l j J 
52 : = (jcC33-jcf C3: )*<JcC33-jcf C33 5 ; 
if (si <=0. OOOS) then st ep ! --st ep*f act o r; 
if (slope <=slperr) or ( (si <-d isterr) 
and (s2 <=-distei'r) ) t iien goto J 00 


eno ; 

beg lo 

out ; 

converge 


’ cn I'mob 'ent’ bui Ics tne polygon data structure 
procedure f o riYioDject ; 


va r 

X, y, xs, ys, xl, yl, s s reai ; 
1 ; lYiteger ; 

vert ex , voo 3 vi'c s oo i noe i ; 


peg 1 n 

voo 3 nt » — ri 3 1 ; 



new ( VO' rtex ) ; 
ve r T. e X i 5 “ ve rt e x ; 

."eanln (af i le, ks, ys.‘ ; x ^ -xs; y ^ =y s ; 
for 1 i 1 to n ve rt a o 
Deg 3 n 

if (i <>;}.) t neri vertex .Is-a: 
vertex* . x s =x “ vertex **. y s ==y ; 

if (aos (x) > 1. then vertex - rheTai :~arctanj (y/ x) 

a Ise ve r bex '* - 1 het a s -1 . 57079G33 ; 

if <x<OhO) tnen vertex t ^letas-vertex* t he?tax-3Wai59i’^ 
else 

3 f Cy <0« 0) t nen ve rt ex * • t neta s =^ve r bex-'*. t he-ca h- 6- 
vertex-’*, next s =v point ; 
if (lOl) then vpoint *- p rev 2 “Vertex; 
vpo int 2 “Vertex ; 
if ( 1 < > nve? rt ) t nen 
begin 

read In (afile, xj,yl) ; 
new ( vertex ) 

end 
e i se 
beg 3 n 

xi s =xB ; y 1 2 “ys 

end ; 

a 2 =■ ( X j ““ X ; * ( X 3 - X ) -H ( y J. “-y > ( y i -y ) ; 

X 2 =xl ;y 2-yl 

end ; 

verlexl \ viext 2 =vpoint ; vertex J 
vpoint*’ « prev 2 “Vertex 1 

end ; 

Psf o riYiODj ect ’ builas the data structure representing tne hypotnetical 
Cl rcl es 

procecure psf o rmoDject ; 
var 

X, y, rad, r 2 real ; 

1 : integer ; 

vertex, voo 3 nt : pointer p; 
beg 1 VI 

vpoint s =vi3 1 ; 
new ( ve rtex ) ; 
vert exp 2=vertex; 
re<aaln (af i le, x, y, rac) ; 
for iS“i to npseucs ao 
Deg in 

rS“Sqrt (x*x4-y-)#-y ) ; 
vert (ax*”'- r s - r ; 
ve rt ex ”’* . raa s “ r ad ; 

if ( aos> (x) > 1. Oe-3) t he)"t vertex t beta s^arctan iy/H) 
else vertex - theta 2 ~1. 57079633; 

if <x <0. 0) then vertex**-. tneta:==vertex ’**. t heta-^3. 1415^Z^ 

if <y<0.0) then vertex*-, t netas ^vertex***", t heta*t&. 
vertex-"*- next s^vpoint ; 

VDO 3 nt 5 “Vertex ; 
if (lOnpseuQs) then 
begirt 

readivi (afile, xl,yl, raa) ; 
new (vertex) 



end 


€?nc ; 

vert exp - next - '/no int 

end; 

bey .in i^-:<-,Y,ain prog > 

^ ^ ^ ^**='^**^***’#>f**x-**-r-**je*-jt-***-^*****-i< >€•***•>(■ *-¥--v 

yj -ne:g^.t or t ''‘e piane of ivtoveiYient above the gioDaw p-.ane n-o 
onLUYj* -nui'nber of oDstacles 

f./Cr.'* i‘UiYjOer of vertices of the polygonai ODject 

tx 3 , y.i , ipliU anrl <xf,yf,fphi) — initial and final positions of tne object 

iU, j ., x_, lo —lengths or the links of the lYiani pul ato 3 ' 

npsQuos — nuiYioer of hypothetical circles 

liiiYi, u3 iiYi~-lower ana upoer liwits o^‘ the joint angles 

steo move step, gstep —step used to compute the gracient 

cd, c'l,c2 — Daramebei's of the oojective function 

******************-K.^< ****** ****** 

stai't ; 


connect (af lie, ’ ^ nouto’ , ’dat’ , ’ r’ , p) ; 

I'eset (af 1 le) ; 

i-eadln (af j le, yO) ; 

read-Yi (af i le, onuiYi) ; 

reac. i n <> af 3 1 e, nve 3 't ) ; 

reac in(afiie,xi,yi, iphi); 

rearl.'h (af = le, x-F, yf, f onj ) ; 

f oi'iYiobject *, 

new ( ODject > ; 

object ■ . next ! ^ve 3'tex 1 ; 

read}h(£<f I le, nosRuds^ ; 

IDsformi object ; 
new ( ps o D J ect ) ; 

psnbject . x!=xi ; psobject , y — yi i psohject •. t netas^iDni ; 
psobj ect •' . next s •=vei'texp ; 
for i!!=!0 to 4 Qo 
begin 

read In (afile, lliiYii:il,u]imCij) ; 
f cict L 1 1 : =u 1 1 lYi C 1 1 - 1 1 1 lYi E X 1 


end " 

read ] n ( af 1 1 e , 1 0 , 1 : , J. 2 , 133 ; 

J ointspace txi,yi, iplii.jc) ; 

J 0 3 ntspacf? kxfjyr, fphi, jcf) ; 

xg ! (x 1-100. 0 ) *g ra ; yg ! = (y 1-100. 0 ) >i-g ra; rotg s =1 pm *cohve rt ; 

■S(-*****-*"«">f»<-*'**^* < **)(•**■**■*•****•* ** * 4 f’*-*- 3 «-*****-****-»t-»**r^< ^**-****-*r**-it***** 

obstacles being fonvied 

**+■*■)*•)«• !»■***■**•»(•*)(• •-*)(•**}«• •«*■»<■* If-** ***■*•** **•'{•*********)(••**■»*■** **************** 

for i!-=l to onuiYi o o 

begin ^ 

I'eadln (af i le, xl, yl, pni, nvert> ; 
new ( obstacle C 1 3 ) ; 

ODstacl eCil'"'. x! = xi;o Dst acle.i 3 ‘ •. y s=y 1 ; 
obstacleCiD'''. thetas =phi ; 
foriYiobject ; 

obst acleC 1 3 ‘\ next 5 = vert exl ; 

get info (obstacieCil , vertex!, xl, yl) ; 

0 x 3 =t rune ( ( xl - 1 E) 0 - 0 ) *gra) ; oy s =t rune ( (y i — 1 ‘jO. 0 ) *g ra) ; 
beg 1 no ( ox, oy, 1 ) ; 
fil 1 (6) ; 

d rawDo lyg on < cost acle . i 3 ' ; 
endp 

end ; 

ODject X 5 =xf ;o bjBct y s=yf ; ooject-'’. t netas =f phi ; 



Cl raw p 0 L y c 0 n ( 0 b j ect ) ; 

oajf-jct . X : =x 1 ; onjpct . y :=yi ; object •. tneta :=iplT3 ; 
c rawpoi ycon ( object ) ; 
read 1 n ( af i 1 e, cO, c l , c2 ) ; 

read :.ri v af i i e, step, gets p, f act or, rot f act I , rat fact 2, rot fact 2, si oe r r , dLi'Skt-*’) 
read In (af i le, approach, diet 1, oistZh ; 
ci st obet ac 1 e ! = T 3 ; check s =t rue ; co unt s = 0 • 
y rad lent i jc, g rad, slope) ; 

for i:-i to n do jc f 1 3 s =jc Ci 3 -step*a rad [ i 3 ; 

dfp; ■ i 

for i: = l to n 00 sCi 3 J =-g raa Ci 3 ; 

reoeat 

for i:=l to n do jcl Ci 3 !=jcCi 3+step-^sr3 3 ; 

grao lent (jcl, grao, slope) ; 

for i:=-l to n do s l 3 3 ! =s L 1 3 -g raa C i 3- ; 

si : =no rm <s) ; 

for is = l to n ao 

itC*'!:-* jcic] 4Sttt>*sC*3 
t^vdj 

coonirj,covt\V+ij 

({ceutkfc tndd then ch«.tk;s-U«t else cKwVufftlstj 

Unfel 

toot 

£nd* 



APPENDIX B 


'■'SOGRAM HvlPLIIi-’ENiTTIVG 1 i-iE CuMFIGURftTION SPACE ALGORiTHi'' 
ui'Otjraiii av 0 ’ cob«;b ac] e U nput , out put ) ; 

C Dr'lS'b 

coriVff ri: — conversiovi factor froi>i radians to degrees 
iTjaxt'--:, lYi:* r'K't' --arbit rary large and siYiall covistants 

^ •J«-******)f*****^’)#-*-J<--}^***« ******* ){.******-#*•**•******-******•* 

convert=.57. 29577951 ; ^ 

g ra=68» 2667 ; 

lYiaxct™ L - Oe+S ; iyh Ynct-5» “ 

rj i =3 m i^ j 5927 : 

step=0.. 5; 

type 

* Jf-* *** ^ ** ** < *** **************** J^^****************^ 

po lYTcer- -node represent lYig a vertex of a polygoYi 

po3 ygori— -header of fcne structure representing a polygon 

st ree — noce of bne path list 

gpo inter — node reov^esent me the vertices of the 3-d expandeo oostacle 
QDolygon — heacer for bne 3-d obstacle 

M**** ** *** *** *** ************ **************** *********** ****************** 

po int er-=:""node ; 
node=raco rd 

' x,ysreal- 

next, prevs poiYiter 
end " 

Dolygon-- ' 1 f raifie; 
i f r ame- reco rd 

X, y, theta 5 real ; 
next i pointer 

end ” 

sb ref-^~ t reept ; 

I r 0 a 0 1* r r-/ c o V' cl 

X, y, pni : real ; 
son, parent •st ree 

end ; 

cj po int (-•r=' gnoae; 
cncirJe— •» eco rd 

x^ys array [-50- . 501 of real; 
next, p rev s gpo inter 

end ; 

epuiy- ’c3 f raiYre; 
cl f i“cH{Yi£-=ir eco ro 

X, y, thetas real ; 
next igpointer 

end ; 

J,* ^ X * y )e*jt******* *******************‘*****^****^^'><'*******'*^**** 

Q 0 lost ac I ©— — a r ray of oostacles 

.y.^^,^**^**^*****************************'J<''*-******-**-J«********’*'**^'^'^'^ 
phi , marg in, xiYiax, xiYiin, ymax, yinin, t hetaiYiax, t hetamin, xf, yf , fphx, 
xi , yi, X pni sreai ; 
dtheta, xo, yo, factors real ; 

tsl ices, ox, oy, i, k, onum, o, nvert, vwobj, ndraws integer; 
vert ex 1 s po int er ; 

aoDstacie s ar ray Cl»«iOl'of gpoiy; 

gvertexl sgpointer; 

oDject , oDstacies polygon: 

af 1 les text ; 

root, leaf sst ree; 

feas I bles boolean; 

^include < pas >ndsup spas 



■■1 2 O ( pa-- ) SCagiYUr-nr 5 paS 

''-v-iFr ci-^po'aes off tne nodes of a cii'culat' list that is not needed 

j • -D-re-: .n o ~ I'ee ( ve n cex s po 3 nc er i ; 
va r 

ve r i; ex 1 ; p 0 1 nt e i' ; 

Tj o Q ; n 

v©*! tex DTBv' . nextr s -ni L ; 

wrii^e (vertex .. nextOnil) co ^ 

oeg xn 

ver*! ex t !=vert next * 
d ispose (vertex) •; 
vertex s -vertex! 
end * 

d ] ?> pose ( ve ri ex ) 

end : 

"freetri^e’ cispofses off tne recundant nodes of tne oatnlist 

» X X"X*-X* X -5^ X -X-* K‘ X*-X *** X X-X-X-'X *^t*-X-*X”X**X**X'* •X-fr-X’-X'-X' X-X-^»'T»- 

proceuure f reet ree ( noae s st ree) ; 
r 

nod Pi, ) lod e2 s st r ee “ 
beg in 

nod el s ==nocJe‘'« sor^ ; 
t^hixe (nocelOnil) co 
beg •! n 

nodel's -v'lodei *.. eon ; 
dispose (noael ) - 
node*'! s s-^nodel' 

en<3 ; 

o d e . s 0 r j s = n j 1 

end ; 

A-X-X-X -X ^X-X XX--4>--X + X-*-*-^i-X''XXX--X--X*X--X*^**-X**X***frX-*-X-fr*-X*-«-*-X-X-«*-X*-X- 

’getxnfo' o-ji^'iout es tne cloDal coords ’< x, y) ’ of tne vertex ’vertex’ 

083 1 onrj 1 ncj to if* «e po 1 y o on ’ o Dj ect ’ 

V-Hf. V ^X-X <f -^•X-'X' X-X-'X-'X* x-*<t-x--x 8X--)8 XX-* XX-X--X-X-XX’ -X'^*X*X“fr**X'X-*fr'X‘X‘X‘’X'*X'X'**X'X'X***-X'*-X**X'*X**X-X:X-‘X--X* 

propecure get info (object s polygon ; vertex 5 po inter ; var x, y s real ) ; 
v£\ r 

raa , phi , x.1 , yl , x2, y2 2 real ; 
beg in 

x:i a -object . x;yl s^objecf y- 
x2 a ^vertex*’*, x ;y2 2 -vertex*'- y ; 
rada-sqrt ( x2*x2+y2^*-y2) ; 
if (absCx2) <=iYiiv)cb) then 

if (y2<0. 0) t^hen pni s=3- 0*pi /2« 0 else pnit=spi/2.0 
else 
begin 

phi s-arc'can(y 2/x2) ; 
if (k 2<0«0> then phis=phi4-pi 

end ; 

pni s=phi'+'object***- theta; 

;< 5 = x i*+Taa*cos (pni ) ; 
y ; =:yi -8 rad*J^‘sin (ohi > 

end ; 

X# X-XXX'XX48-X*^X’'X*'X*-X'*** XX-X-XX-X-X- 5«‘X**XXX-XX-X'X**X"XX-*'X-X‘if*-XX*'XXX**X*X*X-X 

’:ineto-«+'’ coiYiDutes tne distavice of the point ’ (xc,yc)’ from the line 
ino the boints Mxa.ya;’ end MKD,yD)’-The distavice is given an aroit 
larce value ’maxct’ if tne perp- from tne pt- to tne livie does’ nt fal 
witliin tne segment 



riinct xon i 1 ne^oDi: > xa, ya, xb, yb, xc, ycs real) : real ; 

Vc I 

1, d 1 , d 2, cobJ , cob:., a, o s real ; 

Dt?c in 

i ! ■=** ( X a — X o ) -^ ( X a -“ X D ; “I- < ya ‘*" y d ) * ( y a- yd; ; 
ciX =•**•" < >^a*“-xc ) * ( xa-xc) + (ya—yc) * (ya~yc) ; 
a 5! ^ X I - K p ) ^ ( X o - X c) -I- ( y b—y c ) *■ ( y 0“ y c } ; 

*11“ (aDs<cl) <fYiinct; or iabs(d2) <iYiinct) then 1 met oot s—0- 0 

% 

□eg ).h 

cos .1 u-dl'*- 3 -d2:;cr;sJS-=ar:H*'i-dl : 
a ! “ 2 « 0*so r t ( d 1 1 ) ; d s —2- 0*sq rt ( d2* 1 ) ; 

if (aos ( 1 , 0“"Cosl /a^ < mi net) or (aos ( i - 0-+*cosl /a^ (mi net; thcY> 
3 met oot s =0. 0 
else 
beg 1 n 

if Cans ( CO si /a; <iYimct ; t nen i 3 net oot • =sq rt < a I ) 
else if < aos <cos2/b) <iYimct ) then i i Yiet opt s =sq rt (dlZ) 
else 

if ((cosDO-O; and (cos2>0, 0;) then 
] met oDt 2“sqrt (c2-“cos2*cos2/ (a^l) ) 
else 1 3 net oot s==iYiaxct 

end 

ena 

end ; 

’max’ cnecKs if ’t’ is large (maxet) or not 

fuY3cti0 3i max 0:5 real) SD 0 0 j.ean; 

□eg *1 n 

if <aDs <t- jYiaxpt ) <100-0) t neri imaxs-true 
el se max s = f al se 

end ” 

’ a rccos’ cciYiputes tne cosine inverse of ’ex’ 

f unct 1 on -rtrccos (cx s real ) • reail ; 
v/a r 

5x , a 5 real " 

□eg i n 

jf < aos ( cx - 1 - O; <iYi3 net > t len arccoss~0-0 
else 

if (aos (cx-{* 1. O) <iYimct; tnen ai'ccoss-pi 
e 1 se 
□eg 1 n 

sx s •’-sqrt ( l“-cx*cx) : 
if (aos <cx) >minct ) theYi 
beg 3 n 

as-arctavi (sx/cx) ; 
if (cx<0-0) tnen as^a-J-pi 

evid 

else a •* SSI p 3, / »* O 5 
arccos5=a 

end 

c-nd ; 

*■ f ■>:**■>■****■*■*■***** >■*****■***•¥ ****’<^******i^***^************* 

’formobject’ rorms-the dai^a structure representing a polygon whose 
posit ■> on is given by ’ (xo,yo,3hi)’ 

^*^*^^***;^* 5 , i**^+^(.***********»**********»**»+-»*»********************* 



•jrc^cecjurfc) f oriYioDject (xo, yo, phi s real; ; 
va r 

H, y, r, ;:i s real ; 

1 : integer; 

vertex., vao 3 rt r, poi hter; 

□ eg 1 n 

V 3D int .1 -ni 1 ; 

new ( ve r-cex ) ; 
ve r 1“ ex 1 : =v€5 rt ex ; 

roaci n (afiie,x,y;; v 

for = l to nvert do 
beg i n 

if (aos (ohi; <iYi3 net ) tneri 
beg 1 

vertex - x s -xo+x ; vert ex*'*- y s =y o+y 

ena 
else 
Deg 1 Y’l 

r s=sqrt \sq r (x) -J-sqr (y ; ; ; 

*if (aos*. x) ^Yiinct; tnen 

3f (y<0.0; b«ieri z i J - 3 - d 3 /2- 0 else 
else 
Deg in 

zi JJ -arc ban (y/x ; •, 
if <X<O.Oi theYI Zl2-=Z2H-pi 

end ", 

z i S“r i“f"phi ; 

vertex - x s -xo-*- r-^cos \ z i ; ; 
vertex y 5-yo+r*^si yi ^ z i ) 

end ;; 

ver tex* *, next : - vdo int ; 
if (i<>l) t lien VDoiYit prevs=vertex ; 
voG 1 nt s =ve rt e^x ; 
if ( 3 <> nve r t ) t hevT 
□ eg 1 n 

read In (af i le, x, y ; ; 
new (vertex) ; 

eno 

en«“j “ 

ver lex j « next s “vpoi vit ; 
vpoint * . p rev s =vGrt ex 1 

end ", 

’ Dsf c« riYioDj ec b ’ expancs tne polygon ’object*’ by ’tYvargin’ OY*y all sioes 

proceeJure psf o riviODj ect (object £poiygon;iyfargin5 real ) “ 
va r 

o, piTi^ r'J, rx~>., X L^y ^ ^ x^*-^ y^ >j x>.>., yv».»^ x^i, raoo** 2 eal , 
vert P-3X, V 30 i nt , pvert ex s do inter - 
enters boo .j eari; 

□eg 1 ri 

vertex s^object ’•» next ;eYiters-falsc; 

iiew ( pver bex) ; 

vertex! s -pvert ex; 

vpo i r*it s ~n.i 1 ; 

repeat 

xi s —'vertex ' * x^, yi 5 5=vertex*’% y; ^ 
x25=vert<ex *•. next**’**, x ; y2s ^vertex”", viext**’'*. y; 
x3 2«::vertex-*\ preV‘% x ; orev**'\y; 
rX £ = (x 1 “*x 2 ) *(xt’'-x2)'<-(yl-y2) •^(yi-y2) ; 



r 'tr ^ X : -x.'i ' ^ (xi-xZ) f* t y I -y3 ; ^ iyl-yZj) ; 

r3 5 •* x?*^) ^ ( x2--x3) -h (y2“>y3) ^ (y2*-y3) ; 

p '"I : s -a j cc o s ( ( r 1 +• r2~ r 3 > / (2. 0 ^sq r-c ( r I * r2 ; ) ) : 
c : -1 /co<'-5 ( p-ii /2- 0) ; 
pni /2, 0? 

radd i=iYiarg in/cos (ohi ) ; 

x2s - xl-t-a* ( x2-“>< 1 / / SQ rt ( r J. ; ; y2 : -y *■ d-^ (y2-~y i ; /sc r b ( rl ) 

x3 s =x < x3-x 3 ) / sq rt ( r2) ; y3 s =y (y3“y i ) /sc rt ». r2) 

x4 s - Cx3*+*x2 ) /2. 0 ; y4 s = ^y3“^y2) /2. 0 ; 

pvertex *. k s=x 1 +racd* (xl-x4) ; ^ 

pvertex*’. y s =y t-nracd* <y 3 -y4 ; ; 

pvertBx *. Yiext i!5=vpo in-c ; 

if (enter) tner« vpoint •- prev J -pve rrex ; 
vpoint s=pverrex ; 
ent ers — t rue; 

Vf^rrex 2=yertex* prev; 

if ( verl* ex <> oDject -next) then new(pvertex> 
unbil ( V 0 rtex=object -next); 

vei^tex 3 next s=vpoint ; vpoint *•« p'^cev 2 "-vertex t 

enr •; 

p r D ce( I u r p d . a wci r c .! e ( >< , y , r : r ea 3 ) ; 
r 

X 1 , y : s integer : 
tnfeca, dtnetas real : 
peg L n 

L -let a 5 O : a t net a 2 -O. 3: ; 

y 1 ;--t runc^y^ ; 

X ' "'t rime ( X’t-r ) ; 

lYiove ( X 1 , y *1 ) ; 
reoc^at 

b betas ~*c neta-+-aT; beta ; 

X 1 s “= 1“ rune ( x + r^f-cos iz beta) ) ; 
y 1 s - t ^ unc ( y + r 5 1 n \ t bet a ) ) ; 
cj raw ( xl , y 1 ) 

un I" \ \ ( t beta > =6- 283 i 854 ) 

evu: ; 

procecj ii ro c rawpo lygon ( ooj 2 00 iygon) ; 
va r 

t bet a , X , y 5 rec\ i ; 

* xi , yl 2 integer; 

vert ex s po 1 nte r ; 

ben 1 n 

ve r fcex s = 0 bj next : 

if ( 0 o j *=0 o j ec b ) t neY"! get inf 0 ( ooj ^ vertex . x, y ) 
e 1 se 
begin 

X i srvert ex *'* « x ; y s =ve »'t ex ' - y 

end ; 

X 1 s =:t rune ( (x- LOO - 0) -i'gra) ;y 1 s=t rune t (y-i 00. O) *q ra) ; 
if (xl<0) then xls-0;if (yi <0) then yis=0; 

1 ( X 1 > 4090 ) t nen x 1 s =4090 ; 1 f ( y i > 3u8o ) t nen y I s ^ : 

iYiove(xi , yi) ; 

repeat 

vertex s ^vertex ’ . )iext ; 

if <oDj=object) then get inf 0 (odj ^ vertex, x, y ) 
else 
begin 

X s-vert ex*^*- x ; y s-vert ex*''", y 

ena ; 

xl!=--trunc( {X“100,0J*gra) ;yl s=t runc( <y-100. U>*gra> •, 



11 (xl<o) then xi:-0;if (yl<n^ then yls^O^ 
if (xi/'40SO} then x J. : 090 " i f (yi;3080; then yis=3uB0: 

raw < X 1 , yl ) 

wt 1 1 (vertex-oDj . next ) 

enc ^ 

.pro ced l; t r Rce o o j ec b ( rv , 1 s i nt eg e r ) ; 

va r 


ve j'tex s goo inter ; 

X, y : real ; 
beg in 

vertex • ==gobstaic3 eu3 3 . v^ext ; 
ODject ‘h t neta: •=k*dtneta 5 
reoc^at 


X 2 -vert ex“**« xEkj ;y '.-vertex . yCkli : 

ODject X s=x ; ooject •> y»-y; 
a r«i w oo ] yg o n ( o b j ecu ) ; 
ver t ex 5 — ve rt ex .next 
unt ,1 ] ( ver b ex-go bstac LB C i 3 * « viext ; 

end 

’wall’ cnootii-s if the point ’ (x,y)’ is within tne specified wornspace 

^ -jn-fr-K ****fr***J( -if* \ J#* Hit -K ¥i 

"“onct ion waf'l l x, y : real ) r. boo lean ; 
beg i n 

if x> “=xiYia X ; or (y; - yiviax) t herj wax 1 1 s -t V'ue 
e i su 

if ( X <==:xiYii n) or (y<-»y}Ynx-0 tnen walls -true 
else walis-false 

end ; 

procedure b r acc-*? ( YioaeJ , node!' s sc ree) *; 
va r 

xa, xP, ya, y L ; d , dx,, oy , anhin ap ii , oohi s real ; 

1 , nd raw s integer ; 

Decj 1 n 

xa s "-nodel •« x ; yas - node: ‘ • y ; aoni s^Yioael **« phi 5 
X D 5 -noce 2 *'*- x : y b ! =Yiooe2 ' - y ; bphi s »snode2 ’**. pin ; 
ds-sqrt (sq r \ x b-xa; -i-sn r < yb-'-ya) -i-sq r ( Dnhi-apni ) ) ; 
cx s-(xb-xa) /a ;dys= <yD- ya) /a;dpni s- ( bph i-apni ) /d; 

'r'lC raw 2 -"t rune ( a / 1 - 75 ) 5 
for i;==i to ndraw go 
beg 1 n 

xas=xa-i-i. 75 ->tdx;yaJ-ya+l. 75 *cy ; aohi ? =aDni + l . 75 *dpii 
object-’-. X :=xa ;object-’ . y !=ya; object”-, t net a s -=3 phi ; 
d rawoo lyg on ( object ) 

end 


end ; 

p r o ced u re a b o rt , 
beg 1 n 

W('-| teln t’ Searen aoorted.Patn does not exist.’); 
goto iOO 


end f 

’ "ucnkint’ fieteniyiities tne points of intersect i on of tne line joining tnft 
points Mxa,ya^’ and ’ <xd,yp>’' with tne ’k^th slice of the 3-a 
Obstacle ’gons’.The pts. are specified in terms of ’tl and t^ wnere 
tis-'O' is (xa,ya) and ti=-l is (xo, yo) 

proceaui 


IJiB g QuS » i J iwr p Cl * I” ... . -v- w 

is (xa,ya) and ti=-l is (xo,yo). 

l.:^^.^,:)f,it^# 1 i..i^^*******^********************************************** 

ire pc-iki nt < xa, ya, xo, yos real ;gobs«gpo-ly;K s integer ;var t i, tU! real ) 


var 


a, b, c, a, xc,yc, xd, yd, t , aen : real 



if (xl <0) then x\s*=0;if (yl<0) then yls-0“ 

if < X 1 > 409^") ; i nevT x J “• =■ 4 090 "if ( y 1 > 3080 ) z nen y i s ~3o8u ; 

»’ • raw ( X L , y 1 ) 

l\M t 1 (vertex-oDj • next ) 


eriC 


procerlur.' r » r-cpcojc-?ct ( 1 s integer) 

vr-> r 


ve I' l ex « g D n 1 ni <-* r 
X , y 2 rea i ; 


Deg 1 1’l 


v^e r I ex u ==g o bst ac3 e ^ 1 1 - viext ; 
object M l“‘*>eba! -k^(jthe^ta ; 
recu-^at 

X 5 -vert ex ‘ « x L k j ; y ^ - vert ex* « y C k J 
oDject* . X s=x •; ooject-* - y s-y ; 

Q v£^woo ’I ygori ( o bject ) - 
vert ex 5 -ve rtex' .next 
uvni .1 1 ( verCBx-gobstac le Ci3 .next) 


enc - 

^wal'i’ if tno Doint '* within tne specified worK'r^pace 

function wa"!. 1 <; X, y 5 real ) s boo lean; 
beg *1 n 

if (><>=xiYia\x) or (y;-yiYic\xi Inen Wciil5=t)ue 
else 

■i f < X < — x«Yi in) o r ( y < — yjYn n) I nen wai 1 1 s — t rue 
elee wali2=false 

sYid ; 

procedures trace (nodeJ, nodel* s st ree) - 
var 

xa, xb, ya, yb, d, dx,, dy , aohs , apni, Dohi i real ; 

.1 , nd raw s in teger ; 

Decj 1 n 

xas -nodei* . x ; yas =^node:-“ - y ;aohi s«noael*\ pnx ; 
xos=noae2 \ x;yb5 = noae2 •.y:bphi : ^nodeT:;**-. phi; 

/;•; s srcjQ rt (sc 4 r ( X o~xa; -esar (yb-ya) -i-sq r ( oohi-- apni ) > ; 
ox s = (xb-xa) /a;dy:- <y3--ya) /a;dpni ::= ( bpni -apni ) /c3 ; 
nc raw s-t rune <d/l- 75) ; 
for ii-i "CO ndraw co 

xas-xa-ki. 75 *dK;yai-ya+l « 75*ay ; aohi J-aoni-M- 75‘5tdpni ; 

object-. x:-xa;object-.y:=ya;objecr-.tnBta:-aphi; 

drawDO iyg on (object ) 

end 

end ; 

procedure abort; 

te3 n(’ Searci aoorted.Patn does not: exist. ); 
goto iOO 

********^*****-)«-***>-i-»-»******-^^**********'****** 
***#*# >^**-»**»f+^-^***-^** ■ * ao^rrs of intersectiovi of tne line joining tn* 
^pchKint’ determines J ^ wlt^^nL ’k”tn slice of the 3-o 

poi«r. Mxa.ya, ‘ 

oDStac-e ’gobs’. !i'ie pts. s> «- i5pt=<-.v 

ta =0 is ,""»t«l,il,»l»’.i««»**»****»*»****»**»************ 

g„U,:gpp.ly;K = .nteoer;var tl,t2=rss.> 

proceaure pchk int ^xa, y^, y 

a, b, c, d, xc,yc, xd,yd, t, aensreal ; 



yaiiSDoolean; 

Dsg 3 n 

tl 5"maxct ; t.2:~0. 0-maxct ; 

iF (aph I -bphj ) <=0, 5*dtlieta) tiien 

'jeg 1 n 

k s -get k (apni ) ; 

pcfjJMrrt* ya, xb, yb, gods, k, t 1, t2) ; 

enc 

loeg in 

J - =0 ; 

a i — BQ t ( sq r ( X b- X a ^ -1“ so r ( y o— y a ; -i-sq r ( a o h i - d p n i ; ) i 
d X ? =: ( XD— xa) /d ; (Jy 5 “= (yd -ya) /d ; dpn i s = ( boh i -aphi ) / a ; 
i :=Btep; 
repeat 

X i =Ka*‘(*'t *Gx ; y : -ya-^t *oy » pni 5 -a\pna -f-t pni ; 
k s-getk (phi ; ; 
pt 3 npo 3 y ^ K , y, k, g 0 bs, yan) ; 
if (yan) then 
beg 1 n 

Qi s =- (t -step/I;. 0) / a: 
if ( d 1 < b 1 ) t hen 1 1 - — d 1 ; 
if (Gl>fc2) then 

enc ; 

1 3™b^-st eo 
>.int 1 1 ( l:>=d) ; 

if jYiax (-tA ) then t2S“iYiaxct 

end 

end ; 

’ adi'jevxj’’ aoQ'^ to tne oatnlist^any new feasiole point on the path that iS 
oot ^Hioeo 

procedure aopenci (anodesst ree;xb, yb, ophi s real ;var t reenooesst ree) ; 
deg iri 

new (t reenoce) ; 

fc reenone"'- x s = xo ; t reeviode'***- y s —yo - 1 reenode *.» pni s = ophi; 
treenode -sons -nil; 
anor..e* - son s=t reenode ; 
r reenoc'e"**. parent s-^-sjanooe 

end ; 

’repetition" checks if tne new ooint ’ <x,y)’ obtainea is a repetition 
an earlier stcite of the object- ’ 'node’ is tne last noae of the pathlist 

’f’unct 1 on reoet 1 1 ion (x, ys real ;nodei st ree) s boo leavi; 
var 

xl, yl, x2,y2, dSreal^- 
terjYj 5 DO o lean ; 
becjivi 

xt s-node**"- x;yl s-node* • y ;tems-false; 
whi .e (nooe-“\ parent Oml ) aYia (not (term) ) ao 
begin 

x25«ftnode‘*- parent •“% x ; y2 s -node* *•- parent"--/; 
d 5==! inetopt (x 1 , y 1 , k 2, y2% x, y) ; 
if <d(«margin) then teriYjs-true 
else 
beg i n 

xl s-x2;yi s«y2; 
nooe 5 -node*"**- parent 



epd 


end ; 

3 t j o r’l s “t e tiyi ; 

r?nd ; 

p r r» ced n re r€»'X r ac e “ 

V a 1" 

t ri ( » c e s s t >'ree •; 
beg : n 

t riDce " -TO ox ; 

w'nle <tnc)de* sonOnj. x) go ^ 

deg 1 n 

t rc\ce (tnode, bnoae - eon) : 
tnoae s =tnooe **- son 
end 


0]-»c ; 

iYj DC I Pyt ree’ deletes axl tne nodes between ’noaex’ and ’node2’ in tne 
’ Dctt n ) ist ’ 

p rooec u re »r»od x fyt ree (noce‘1 , noce^s st ree) i 
deg 3 n 

nodel* **M ::i3Lrer\t . sons-ni 1 ; 
f ree t ree (nod el ) ; 

nocler M 5ons~v*^ode2;noGel: '. parent s -nodei 

enc ; 

’ reduce^ refines the path from the point given Dy ’anode’ of tne patnll^ 
bo t*sab givev"! oy ’ bnade\ ’ crjoae’ oeing the intermediate point. 

*^.V X •X-*-X*-X**X* ¥-¥M‘-¥:*’^¥r¥r¥r¥‘¥r¥:M‘¥‘¥¥‘^¥r¥r¥-¥‘¥r¥r¥r¥^¥-¥:¥‘¥r¥‘¥r¥r¥:^¥r¥h¥r‘fr¥: 

d-'-Dceaure reduce (anode, cnode, bnodesst ree; ; 


va r 

viodel , nodel's sx ree* 
yes! DO 0 lean; 

xa, ya, apni, xb, yo-j do hi * real 5 
procecu re chKsafe (xa, ya, aphi, xb, yb, ophi s real ) ; 
var 

1 s integer ;; 
t 1 , t 2 2 real ; 
oeg i n 

1 s-*! ; yes 2 -true: 

repeat , . 

gchKint (xa,ya, apm, xD, yo, do^ti , gobstacie tiJ 

if (tl<1.0) then yesi=false; 

1 2 - 1 + 1 

until ( i=oTv.iiyi+l> or not (yes) 


1 1 , 1 2 ) ; 


enc ; 

beg in 

oodel !=anoce; 
repeat 

if (nodel-anoce) 
HA i =inQael'''. x ; ya s 
repeat; 

xQ! ==node 2 ' 


tnen nodel'J = bnoae-'. parent 
=nodel ■■'. y ; aohi :=noael''-. p'ni 


se 


niOce 2 !=bn 64 * 


unt i i 


x;yos=nocie2-'^. y; ppni ;*noae2’''. 
chKsafe (xa; ya, aohi , xb, yb, Dpni ) ; 

if (yes) then mod ifyt ree (noael , noae2) 
else nodeZ 5 ==nQde 2 -''. parent 
until (node2*cnoae) or' (yes)*, 
nodel • =»nDdel ■"'• son 
(nodel^cnode) or (yes) 


phi ; 


end 



^ ^ + ^ w * ^ ^444*4 44K-4444**4**44.»44 444 44* 4********* *4 

’5.i'<--:.r.c-^-'V searci-'es vhe livie joining poinbs ’ (xa,y 3 )’ and ’ <xb,yi 3 )’ 
p»! L 'C- n.ai’tr' u I '& ■<’’t,i s.ices for the 3 nteriYied i ate ooints on this 

,X*4.-4*4*4. *4), 44, 4*4 44* *444**4*****4 4*4 *4**4*4**4*4*4 *4*4 444****4*444 *4* 

■I'oceu u I u' anK 1 (anoce 5 St res; xa, ya, xb, yb: real ", k. 3 : i nt eger ; va r ta,tb 

Vet r 


^‘--5 yc, xr , yo, fc 3 , t r.\ dc\, d ti J real ; ' 

i : integer; 

'seg 3 n 

t3a5 --lYiaxct ; Qo: -iffiaxct ; ' 

ocniMnt '>xa, yax, xo, yb, goDst aerie L33,K,ta,tD^ ; 
ro r t 5 =1 to onijiYi c.o 
if <3 011 then 
beg 3 n 

pchk lYYt <; xa, ya, Kb, yta, go ost ac3eii3,K,t:,tl'>; 
if not (max <t I ) ) and not (max (t2> 3 tnen 
beg 3 n 

if (ti<ta) and (fZitb; tnen 
begin 

t a ! -t 1 ; t o 5 =t' 2 

end ; 

if (tl<ta) and (t2<tb> and (t2>ta) tnen 
if (tl>ta) and (ti<tb) and (t2>tb) then tbi*Vl 
if ( 1 2 < t a ) t nen 

if (ta--bZ'<daJ tnen dri!=ta-t2; 
if (tilth) then 

if (tl-tD<db3 tnen dbs-ti-tb 

end 

end ; 

if not (max (oa) ) then ta ! =ta-aa/2. 0 e . se t a s =ta-5. 04iYia 
if not (max <db) ) then tb!=tD-i-db/2. 0 eise t b !=t b+S. 0*maY^{nj 
KC! =xa-i-ta* (xb-xa) ;yc s *ya+ta* (y b-yai ; 
xd ! !=xa+to4 ( xb-xa) ; yd : »ya+t b4 (yb-ya) ; 

if C wal 1 (xc, yc) ) or ( I'epet 1 1 lovi (xc, yc, anode) ) tnen ta! 
if ( wai 1 (xd, yd) ) oi' ( repet 1 1 ion (xd, yd, anooe) ) then to! 


end ; 

44 ** 4 . 4..4 4*4 44 44**44444 * 4*4 444 44 ** 44 ** 4 ** 4 ***** 4 * 444 * 4 ** 4 4 * 44 *« 4 ** 44*****4 

’ fo’ IS tne fincpath procedure that calis itself 3 'ecursi vely. ’ an’ is the 
last node in tne patnlist and ’ ( xb, yb, bom ) ’ gives the position of the 
object to vjhicin a path is to be found. ’ (x r, yr, rphi ) ’ is the position or 
the lYiiobt- of intei'sect lOYi winh the closest obstacle in the earlier 
recursion steu. ’w’ tells tlie sioe of tne normal plane through (x r, y r, 
o‘n w n X c n “t i n*t €? rjYiBd 3. © pt u 1 1 ©s» 

*4. 4.****.4 ***** **** 4 *4 ***** 4 ** 4 ** 4 * 44 *** 4 ***** 4 ****** 4 **** 44 * 4 * 4 * 4 ******** 

px-ocedure f D(an! otree; xo, yo, bpm , xr, yr, rpni s real ; wJ integer ; var bnsstreej 
var fea ! boolean) ; 


var 

aDhi , pp"ix > y^7 

X, k s xni;eger; 
sea s DODleaT’j; 
procedure pat Hf inc ; 


1 1, t2, t , aist; 5 real 


** -Jf # •X-'Jf -K** * •X-'X 

’ pt -noce 


*ii*I**4******»it44-4*4*4**4*-4************4**44*4**4***4****4 

iom'espnting avT intermied iate point 

"^ii;;**********************************-*-**************^*** 

pt*'''bts; 
pts== record 

x» y> t, pirn ! 3'eal ; 
next ! ot 


end ; 



V <*1 ; 


' ' “V’ , P^H , a, D, c, X, y, X 1 , y3 , d, xs, ys, <^Dni , x , xm, ym, 

<=i , x o ! I'ea j 


» 4‘ ;f it- i « < P » 

' frK-oiy 

r i cj ^“i t " ' r* 0 a e ‘ „ 


^ ^ J > vmoae : i 

i*»e.*r:\(.1 J pt ; 

yer> s Dooleav-j^ 
off a.i tno 


nxetjer • 


j# 

noaes ivi 


bne list of iirceriYiea late pfcs* to ih<. 


^ar-iother node to tnis list in me oraer of increfasina * tc^. 

•K .JH •JfX-'X- X-X--X- X-X-X 

-■'■ oceaui's f ree2 (node : ox ) • 
va r 


noael : pt ; 

□eg 1 n 

w r"j 3 1 e ( n 0 d e 1 > VI 1 i ; do 
beg in 

VTodel : --node . next; 
dispose (noGe) ; 
vnode s —vnode 1 

end 

evid ; 

proc'£?dure aopevndl3si (xc, yc, oni, tcs real ) ; 
var 

node, t vnode:, tvnode2s pt ; v 

term 5 boo lean; ' 

beg i vn 

new (vnode); 

node"*'. X 2-xc;vnoae*‘\ y 2 -yc ;node - pni s = ohi ; vnoce**'*. 1 2 =tcj 
if (neaa=vnil) thevn 
Deg ivn 

head 2 -vnode ; neaa next 2=n3 1 

evnd 

else 

if (tc<head’.t) thevn 
beg ivn 

node*'-, vnext 2=head ; neaoJ =vnode 

evnd 
else 
beg 3 vn 

tnodel 2 =neaa ; terivi s-f aise : 

while (tnodel'-. next 0 ml) and (not (xerm)) do 
begin 

tnodel.' ! =x node 1 ' . next ; 
if (xc (tnoae2'' . t ) then 
beg 1 n 

tnodel-'-. next ’• =node; 
node' . next !«tnoae2; 
teriYiJ=t rue 

end 

else tnodel 

end ; 

if (tnodel"-. next=mi ) then 
*beo 1 n 

tnodel"-. next : =node; node"', next s mil 

evnd 

end ; 

if (vnnode) iO) t-nevn 
begin 

t nodei Sinead ; 



'^nil.e (tnodeJ -.next .nextOnjl) do 
tnooel :=tnocjel .next; 
di'iposc; (tnoaet next ) ; bviodei next j 

end 

end ; 

P'-oreciui'e c ik;jath (x, y, ph: : rea] ) ; 
van 

cn ! <3 c ree ; 
beg xn 

fo (an, X, y, pni, xd, yp, ppm, I, rn, fea) ; ' 

1 f ( f ea > t hen 
begin 

fp(cn, XD, yb, bpni, xd, yn, ppni, 2, bn, fea) ; 
if (fea) then reduce (an, cn, on) 

end 

end ; 

procedure fc?hecK <xl, yi s real ^var tsreai)- 
var 

X, y, d 2 real ; 

Deg in « 

1 f not (lYiax (t ) ) t hen 
beg in 

K 2 =Kiyrt-t *x 1 ; y 2 ==yiY«-<"t *y l ; 

d 2=d X* ( x~x r ) -i-dy* (y-y r) ^-dphl ^ ( johi — rph i ) ; 
if (d<0«0) tnen t2=fYiaxct 

end 

end “ 

p r D ced u re rec u r s i o n • 
var 

xc, yc, cp'-ii 5 real ; 

Yiode, nodel s pt ; 

beg 

node!=head ; 
if (heacOnil) then 
repeat 

xc2 —node x :yc'2 =node ' - y ; 

CDh i 2 -node***, pni ; 
cnkpat h (xc, yc, com > ; 
if (not (fea)) tnen 
begin 

nodeJ 2 -node * - next* ; 
d ispose (node) ; r^oae5i=noael ; 
f reet rf^e ( an) 

eno 

until (fea) or (node-nil); 
free2(nod©); 

if (node-njl) or (neaa-viii) tnevi 
oegin 

bns-ni 1 ; fea 2 -false 

end 

end ; 

procedure formliat ( x 1, yl, t 2 real ; i s integer) ; 
var 

xn, ynt, ta.» t b, xc, yc, tc, phi 2 real ; 
begin 

phi 2-i^dt beta; 

xnJ — kiyH“xI ;yn2 5=!ym*^yl ; 

auxf indpath(an, xm, ym, xyi, yn, 1 , k, ta, tD) ; 

* if (wOO) then 
beg i n 


I 



fcheck (XI, yj, ta) ; Fch«=?c.-, (xi, y:, bo) 

end ; 

if (v-jot (lYiax <ta) ; ) then 
beg i n 

nnoae s =nnoae“f” 1 ; 

tct =-- 3 q p ^ fact o f*^o r C fc j h r ( t a) ) : 

xc 5 -xiYiH-t X 1 ; yc s —yiYH t y 1 ; 
aDpevicl last (xc, yc, pni , tc) 

end ; 

if ( no t ( jYia X ( t b ) i ) t ncen ^ 

begin 

nnooe s =vinorie-+* i ; 

t c s -sq rt (fact o r^sq r < t ; -Hscr (to)); 

XC5 =X!Yi“i-t D*xl ;ycs=yiYi-htD^yi ; 
aopend] ist (xc, yc, pni, tc) 

end 

end r 

Deg I vj ■Crfr-jC'* pathf-ind 

neac : ~=ni 1 ; nnoae s =0 ; 
if b nen 

IP (w=.1.) then 
negin 

dx s==.xa--xr; dy 5 -ya-y r ; dpni :=apni-rphi ; 

end 
else 
beg 3 n 

dx s "~xD--xr; dy s ~yo-” y r ; doh j. bpnD -roni ; 

end ; 

ai--xD*~xa; b s-yb-^ya; 

c!==sqrb (sq r (a) 4-sq r ( b) -f-so r ( boh 3 -apn^ ) ; ; 
ip (abs (a/c) (I- 0e-“3) avid (abs ( d/c) < 1 « Oe-3) then 
begin 

xl s =0. 0 ; 1 s —get k ( ppn3 ) ; 
repeat 

y 1 s -sq rt ( 1 . 0-sq r ( x 1 ) ) ; 
xms~xp;yiYjn=syp; t S“0« 0; 

f oriYil i st ( X K y 1 , 0» 0, i ) ; f o nvj i ist (-y 1 , x i , 0. 0, i}j 
X 1 5 =xl4-0. 1 
unt i 1 ( X 1 > 1 . 0 ) ; 

recursion . 

end 

else 

begin 

if (aDs(D) (lYiinct) tnen 
beg 1 n 

xi 5=0. 0;y:l 5 = 1. 0 

end 

else 

if (aos (a; <^Yanet ) then 
oeg 1 n 

y 1 s , O ; X 1 5 = 1 • O 

end 

else 

beg i n 

xi 5==!. 0;yl 5=-a/D 

ena ; 

d;=jsqrt (sqr (xD+sqr (yl > ) ; 
xi!»xl/c5;yl!=yl/cj; 

xss^yi* (Dphi~aoni ) !yss«-xl* <ophi-apni ) ; 
sphi ■.»xl*b“yl*a; 



d : > 

><B 

1 f 


“Sort tsqr(xs;+sar(ys;+sqr(sphi) ) ; 

xs/d ;ys!-ys/cl ;sphi :-sphii/a; 

hot: (sea) then 
□eg 1 vn 


end 


XiYi:-xp;yiYii!=yp; 1 :=:Qetk (poru 
f o rml 1 St ( X 1 , y 1 , O. 0 * 1 ) ; 
recursion ’ 


else 

begin 

n!=!k; 

for i!=— cslices to tsiices oo 
Deg in 

k ! =’n ■ 

pni s = i*ctt he'ca * 
c s ~ ^phi-pphi ) /spni ; 
xiYi ! =x D+t *xs ; yiYi : =y p+t -H-ys ; 
pt i npo ly < xiYi, ym, i , gobstacieCkD , yes) ; 
jf not (yes) tnen 
Degin 

kJ=f); j :=3 ; 

wni le (k= 0 ) ana (j\=onuiYiJ do 
begiYi 

if ( j < > n J t hen 
beg 1 n 

pt inpoly (xm, ym 

g 0 bst ac 1 e C J 

if (yes) then 

end ; 

end 


end 


eYio ; 

if (kOO) t hevT 
beg 1 Yi 

X s =0. 0 ; 
repeat 

y ! =5q rt ( 1 . 0-sq r ( x ; ) ; 
f o riYil 1st (x, y, t , i ) ; 
Formiist (-y, x, t, i > ; 

X ! =x+0, 25 
unt 1 1 ( X > 1 . 0 > 

end 

end ; 

recursion 


end 

end j 

begin <•*•**** finapath 

xa !=an-"-. x ;ya!»an-' . y ;aphi : =an-''. phi ; 

dist !«sqrt (sq r (xD-xa>+sar (yd~ya> +sq r(Dphi-aDhi) ) ; 
t ! ■=!. 0;k 5=0 ; 
for is^l to onum oo 
begin 

gchkint (xa,ya, apni, xd, yo, bpni , gobstacie Ci3 , ti, t2) ; 
if (-t 1 <t ) t hen 
beg 1 n 

if < (t2-tl ) *di 3 t <iiftargin/2. 0) then 5ea5=false 
else sea!=truej • 
t5=tl;k!=i; 

xp5 =‘xa+ (t l+t2) *(xb-xa) /2. Ojyps =sya+ (t l+t2) * (yb-yat>/2.0j 



pphi :“rtp'n + (t i+t2' '«• (oahi-aohi ) /2. 0 

end 

enr. ; 

if <H=0) then 
Deg in 

append <an, xo, yD, bonj , on) ; 
object ‘ M X 5“Xb; object ‘ . y ~yb ; 
oojoct « theta s = rDpni ; 

(i rawpolygon (object ) ; v 

fea 3=:t rue 

enc 

eirrie path find 

t?no ; 

g»oj jyjo . jst a c * e for»Yjs the data structure representing tne 3— d obstacles 

X if* i* X ^■P-¥--¥ k **■« ****** ***********-fr ****** 

p r o ced u re c f o nvi o bst ac 1 e ; 
va r 


Yi, 1 s integer; 
vertex, vpoirit sgooiYiter; 
becj iY‘ 


n 5 ‘=nvprt4-nvobj ; 
vpD lY-ii ;=ni 1 ; 

Y\ew (vertex > ; 
g vert ex 1 s -vertex ; 
for i:-j to n ao 
begin 

vert ex*‘‘ . next s -vdo lYit ; 
if (lOl) then vpo int prev 
VDo 1 nt s =vertex ; 
if (iOn) then new (vertex) 

end ; 

gve Y*'texl'*‘*- next a-voo int ; 
vpoint •***.. prev 2 -gve rtex i 


-vertex ; 


enn ; 

3 ^****^ ^ ^ *^*^****^.^*^^t.^*^Ht*X *-«H******************************X ****** 

’ aiopendoost cvcie forms tne envelooe for obstacle ’i’,the orientation of 
ooject beivig giveri by ’ k’ 

**^:.)^.^*^*, 3 «.***.v.** 3 c.*x-.*** ************* *******************************^«-***- 5 ^*’ 3 ^ 

procedure appe*ndoostacle ( i , k s iviteger) ; 
var 

th€ 5 ta, xa, ya, xb, yb, xc, yc, xd, yd, x, y, a, o, c,d, ai, cosl, cos 2 , xO, yO, 
pni 1 , phi 2 s real ; 

vert exa, vertexo, vertex s pointer; 

Qvert ex s gpo lYiter ; 

beg in . , 

di 5 - 0 - 0 ; t beta s=dt Yiet a* K ;xOs -obstacle *- k ;yu s - obstacle* ' . y ; 
object ' X != 0 . 0 ; object--', y :- 0 . 0 ; object"', t beta: =*t beta ; 

vert exa s=>o bst acle-"'. next ; 
xa s-vertexa"'. X ;ya !=vertexa-'-. y ; 

j(i3* =vBrtexa'^« prev''. x ;yfa {^vertexa'". ore-v . y ; 

a!=yb-ya;b!=xa-xb; 

(jj ; ssa*xO+b*yO‘*'X D*ya“xa*y b ; 

vertex s^object-^. next ; 


repeat 

aet inf D {object, vertex, x,y) ;xd s-xa-x ; yo!=ya-y ; 

0 s=a*xd+o*yd■^'XD•^f’ya-xa«■yb; 

if ((c<0.0) anc (d;==0.0>) or <(c>0.0) ano (d\=0.0)) theft 


begin 


d!=aDS { <a->«-xd+’0*yo+xb*ya-xa-*'yb) /sort <a*a-+-B*d/ ) ; 



:f bhen 

Deg in 

xc 5 — -x ; yc 5 =“y ; 
d 1 2=:d ^vnrtexDS -vertex 

end 


end ; 

vey'bex s =ve rtex - prev 
unbii ( veri:ex-object * next) ; 

0 3jF*ct next 2 —ve rtexD ; 
o v(?rt ex : "-Qobetacxe II i J « next ; 

X s==xc“, y 2-yc; 

>'epeat 

xc 2 =xa+x ; yc s -ya-s-y • 

gvev tex \ x CkI s='Xc;gvertex* *. yCklf s-yc; 

Object *’ . x2=xc; object y2=syc; 

get I nf o (object , vert exD'**. prev, xo, yb) ; 

get 3 nfo ( oDjecb , vertexb" - next, xc, yc) ; 

xd s — vertexa - p rev . x :yd :=vert exa**" « orev* - y ; 

sS 2 =sq r ( xb-xa) -‘“sqr (yb-ya) ; 

b s = BC I' •' xc-xa) "«“sq r (yc-*ya> ; 

c5*-=eqr (xd-xa) +sq r ^yd-ya) ; 

li ( xc-xo) -i“sq r (yc-yc) ; 

d 1 : — sq r (xd -xc ) r (yd~yc) ; ^ i ♦ 

cosl <a+b-d; / (2. 0*sart ) ; coe2 s - < DH“C-d i ) / (2- O’^sart ^ J 

ph 1 1 2 -arccos (cosl ) ; phi 22 -arccoe (cos2) ; 
a5-pni J *tphi2 5 
if «.a>=pi) then 
oegin 

vertexb !=vertexD - orev; 
object"**. X2-0. O; object . y s=0- 0; 
get info (object, vertexD, x, y) ; 

X 2«-*-x;y 2--y 

end 

el«3G 

begin 

vert exa s-vertexa*’*’*. orev ; 

xa 2 — ve rtexa". x;ya2 -vert exa* *- y 

end ” 

g vert ex 2s=gvertex'*‘. prev 
until ( g v e r t e X - g 0 bs t a c X © C 1 J - ne X t ) 


end ; 

beam program*-****-’^********’^^ »******«-*^(-»^****** 

onuiYi — nuffiber o’" oostacles 

— mxtial position 
(xf * yf, ^ — final position 

ffiargin — safety margin 
tslices — no. of slices 

dtheta—onentation incre.ment natnlist 

connect <af lie,’ inputl-new , dat , i , D> , 

reset (af i 1©) ; 

read In (af i 1©^ onum) ; 

read 1 n (a f io.©, x i , y 2 . , 1 phi ) ; 

readln<afile, xf, yf, fpni> ; 

iphis = ipni/convert ; fphi s ==f pm ''^onvei t , 



reaci In V Hf 1 le, nvo bj ) » 

^lver b •• -nvobj ; 
f CTirio:)ject (0. 0, 0 . 0, O.O) ; 
new { oDj ect ) ; 
o ij wci . next s --ve rt ex 1 ; 

rectd . (af 1 ie, i n, xmax , ym in, yiYiax , t net ajYi i n, t net aifiax > ; 
read *i n a f i x e , ma rg i n ) ; 
recicj 3)*t(afiie, tslD ces ; ; 

b-ieca.Yii n!=the-:aiYi in/convert ;theT:aiYiax :=t het aiYiax /convert ; ^ 

(Jt heta ! = (t her amax-t her ami yi) / r si i ces ; 

bsl 1 ces ! =t rune (tsl 1 ces/2) ; 
read In (af 1 le, ndraw) ; 

for i:=:l to onuiYi do 
beg lYT 

readlvT (af i le, xo, yo, pni , nve rt ) ; 
phi ! =phi /convert ; 
new ( oDstacle) ■, 

obstacle . x ! = xo ; o bst acl e . y !=y o ; o bst acl e . t hetas^phi ; 
formobject (xo,yo, phi) ■; 
oDstacle’’ . next i- vert ex J ; 

X o ! =ver t ex 1 ' . x ; yo : =vert ex I--, y ; 

ox ! ^--t r unc ( ( xo -100. 0)«gra) ;oy!=st rune ( <yo- 100. 0; *g ra ) " 

sgo pen ( i ) ; 

beg j np ( ox, oy, 1 ; ; 

f li 1 (S) ; 

arawpo lygon<oDstacle; • 
enap ; 

psf ormoDject ( oostac] e, marg in) ; 

■Free (otasracie' .Yiext) jobstacie nexr !!=vertexl ; 

drawpo ] ygon ( o Dsbac] eJ ; 

sgcl ose ; 

gf o rmo hstacle ; 

YiewigoDstacleCii ) ; 

0 obstacle Til . x s =obsracle" . x ;g obstacle C i j' . y s =ODsracle' 
gottstecle C 1 j' •. next !=gvertex 1 ; 

for k!=-tslice5 to tsiices co aopendobsbacle (i, k) ; 
free (obstacle"-, next) ;aispose(obstacie) 
end “ 

readln (af 1 le, factor; ; 

oDject-'". xs=xf ;objecb y!=yf; object tneta!=fphi; 
d rawpolygon (object ) ; 

oojoct---. x!=xi ;object y J--yi ; odjeef’-. tnetas=iphi; 
d rawpolygon (object ) ; 

Yiew ( root ) ; 

root ■■■. X s =xi ; root-', y s=yi ; 
root-'-, son s»ni 1 ; 
root-' . parent s f=ni 1 ; 
root-^. phi ! = iphi ; 

fD( root, xf, yf, fphi, xf, yf, f pra, 0, leaf, feasible) ; 
if (feasible) theY-» 
begin 

writeln(’*-» path' found ) ; readln; 
ret race 

end 

else adort; 
lOOs 

readln; for is-1 to onum do sgdel(i) 



APPENDIX C 


PROCEDURAL SKELETOK OF THE COMFIGURATION SPACE ALGORITHf? 

BIPLilffiNTATION 

program aw id obstacle, 

« 

♦ 

* 

procedure pchkint (xa, ya, xb, yb, real'j gobs : gpoly; 

Ks integer; var tl, t2i real)*, 

(Checks for intersections of the line ;5o3ning (xa, ya) 
and (xb, yb) on the plane parallel to the »-y plane at 
0 ^, with the k-th slice of the obstacle gobs. The 

, y^) are j^ecified in terms 
ot t^ \^ere Xj^ » xa + tj|^(3d>-xa) , yj^ « ya + t^Cyb-*ya)) 

procedure gchkint [xa,ya,aphi, xbf yb, bphisreal; gobssgpolyj 

var tl*, t2t real] 

(checks for intersedtions of the line joining (xa,ya,« 5 )hi) 
and ( 3 {b,yb*|>phi), with -ttie >*D obstacle gobs. The points 
are again given As t^*s). 

procedure f^pCan* stree, 3d>, yb, bphi, xr, yr, iphisreal, 

Wt integer, var b_*streei var feas boolean), 

(a^ is the last node of the path list. A path is to be 
found from point corresponding to to C3d>,yb,bphi), 
(xr,yr,rphi) is the iai«%)oint of the segment of inter- 
section in theearlier recursion stap, ¥ indicates whether 
the path from to (3dJ,yb,1^bi) is the fomer or the 
latter ««^on^t of the earlier recursion step, in which 
the palii has been decoi^sed into two. (xr,yr,rphi) and 
wr together help deciding the accept sliili ty of an 
intereedlate point). 


points of intersections (x^ 


1) gchkint with all obstacles 

2) if (no intersections) then 

(a) append to pathlist after 
Cb) set feasss true) 

^se 

(a) for every allowable pehkint for 
intermediat® points on plane# 

(b) store all intermediate points in a list. 


*1 

X 

Y 

0 

- 

H 

% 

X 

y 

0 

— 


(c) reciarsive call 

X, y, 0 , 3!p, yp, 0 p, 1 , Cn, fea); 
fp(Cn^ 3d), yb, bpM, 35), yp, 0p, 2, bn, fea)*. 
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